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ABSTRACT 


V 

Extended  Kalman  filtering  is  applied  as  an  extension  of  the  Position  Location  Re¬ 
porting  System  (PLRS)  to  track  a  moving  target  in  the  XY  plane.  The  application  uses 
four  sets  of  observables  which  correspond  to  inputs  from  a  fused-sensor  array  where  the 
sensors  employed  are  acoustic,  seismic,  or  radar.  The  nonlinearities  to  the  Kalman  filter 
occur  through  the  measured  observables  which  are:  bearings  to  the  target  only,  ranges 
to  the  target  only,  bearings  and  ranges  to  the  target,  and  a  Doppler-shifted  frequency 
accompanied  by  the  bearing  to  that  frequency.  The  observables  are  nonlinear  in  their 
relationships  to  the  Cartesian  coordinate  states  of  the  filter. 

Filter  error  covariances  are  portrayed  as  error  ellipsoids  about  the  latest  target  esti¬ 
mate  made  by  the  filter.  Rotation  of  the  ellipsoids  is  accomplished  to  avoid  the  cross 
correlation  of  the  coordinates. The  ellipsoids  employed  are  one  standard  of  deviation  in 
the  rotated  coordinate  system  and  correspond  to  a  constant  of  probability  of  target  lo¬ 
cation  about  the  latest  Kalman  target  estimate. 

Filtering  techniques  are  evaluated  for  both  stationary  and  moving  observers  with 
arbitrarily  moving  targets.  The  objective  of  creating  a  user-friendly,  personal  computer 
based  tracking  algorithm  is  also  discussed. 


THESIS  DISCLAIMER 


The  reader  is  cautioned  that  the  computer  programs  developed  in  this  research  may 
not  have  been  exercised  for  all  cases  of  interest.  While  every  effort  has  been  made  within 
the  time  available  to  ensure  that  the  programs  are  free  of  computational  or  logical  er¬ 
rors,  they  cannot  be  considered  validated.  Any  application  of  these  programs  without 
additional  verification  is  at  the  risk  of  the  user. 


IV 


TABLE  OF  CONTENTS 


I.  INTRODUCTION  . _ . 1 

II.  POSITION  LOCATION  REPORTING  SYSTEM  . 3 

III.  MULTIPLE  GEOMETRY  TARGET  TRACKING  ALGORITHM  . 6 

A.  TARGET  MODEL  . 6 

B.  SYSTEM  MODEL  . 6 

C.  MEASUREMENT  MODEL  . 9 

1.  Bearings  Only  Problem  . 10 

2.  Ranges  Only  Problem  . 10 

3.  Bearings  and  Ranges  Problem  . 13 

4.  Doppler-Shifted  Frequency  Problem  . 13 

IV.  KALMAN  FILTER  THEORY  . 16 

A.  THE  EXTENDED  KALMAN  FILTER  . 16 

B.  LINEARIZATION  OF  THE  OBSERVATION  MATRIX  . 19 

1.  Target-Bearing  Measurement  Observables  Only  . 19 

2.  Target-Range  Measurement  Observables  Only  . 21 

3.  Target-Bearing  and  Range  Measurement  Observables  . 22 

4.  Doppler-Shifted  Frequency  Observable  . 22 

C.  ACQUIRED  NOISE  PROCESSES  (R  AND  Q  MATRICES)  . 24 

D.  PROBLEM  PARAMETERS  . 25 

V.  ERROR  ELLIPSOIDS  . 27 

VI.  PROGRAM  RESULTS  . 29 

A.  PROGRAM  LOGIC  . 29 

1.  PC-Matlab  Structure  . 29 

2.  Subroutine  Structure . 29 

B.  RESULTS  . 30 

1.  Target-Bearing  Observables  Only  . 31 


2.  Target-Range  Observables  Only 

3.  Target-Bearing  and  Range  Observables 

4.  Doppler-Frequency  Observable  . 


36 

36 

45 


VII.  CONCLUSIONS  AND  RECOMMENDATIONS . 50 

A.  CONCLUSIONS  . 50 

B.  FUTURE  STUDY  POSSIBILITIES  . 50 

C.  PROGRAM  IMPROVEMENTS  . 50 

APPENDIX  TARGET  TRACKING  SIMULATION  PROGRAMS  . 52 

A.  PROGRAM  DISCUSSION  . 52 

B.  PROGRAM  LISTING  . 52 

LIST  OF  REFERENCES  . S2 

INITIAL  DISTRIBUTION  LIST  . S3 


vi 


LIST  OF  FIGURES 


Figure  1.  MU  Conversion  of  TOA  Data  to  LTM  Range  . 5 

Figure  2.  Bearings  Only  Geometry  . 11 

Figure  3.  Ranges  Only  Geometry  . 12 

Figure  4.  Bearings  and  Ranges  Geometry  . 14 

Figure  5.  Doppler  Frequency  Geometry  . 15 

Figure  6,  Actual  and  Observed  Tracks  . 32 

Figure  7.  Actual  and  Estimated  Tracks . 33 

Figure  8.  Actual.  Estimated,  and  Predicted  Tracks  . 34 

Figure  9.  Total  Target  Track  View  . 35 

Figure  10.  Actual  and  Observed  Tracks  . 37 

Tigure  11.  Actual  and  Estimated  Tracks . 38 

Figure  12.  Actual.  Estimated,  and  Predicted  Tracks  . 39 

Figure  13.  Total  Target  Track  View  . 40 

Figure  14.  Actual  and  Observed  Tracks  . 41 

Figure  15.  Actual  and  Estimated  Tracks . 42 

Figure  16.  Actual,  Estimated,  and  Predicted  Tracks  . 43 

Figure  17.  Total  Target  Track  View  . 44 

Figure  18.  Actual  and  Observed  Tracks  . 46 

Figure  19.  Actual  and  Estimated  Tracks . 47 

Figure  20.  Actual.  Estimated,  and  Predicted  Tracks  . 4S 

Figure  21.  Total  Target  Track  View  . 49 


ACKNOWLEDGEMENTS 


The  author  is  deeply  indebted  to  the  United  States  Marine  Corps  for  the  opportu¬ 
nity  to  pursue  this  work.  He  is  further  indebted  to  Dr.  Harold  A.  Titus,  without  whose 
counsel  and  advise,  he  would  have  learned  far  less.  The  author  also  wishes  to  express 
gratitude  to  Dr.  Mostafa  Ghandahari,  who  showed  him  the  light  in  optimal  control. 

The  author  wishes  to  express  gratitude  to  his  wife  for  her  patience  and  understand¬ 
ing  through  yet  another  rough  period.  Last,  the  author  wishes  to  thank  a  great  friend  in 
Captain  Stephen  L.  Spehn  USMC  for  his  insight  into  what  turned  out  to  be  a  fairly 
complex  problem.  Gratitude  is  also  expressed  for  his  collaboration  on  the  error  ellipsoid 
problem  contained  herein. 

To  his  sons,  Nicholas  and  Patrick,  the  author  dedicates  this  thesis. 


viii 


I.  INTRODUCTION 


The  military  principle  of  combat  maneuver,  in  its  most  basic  sense,  relies  upon  the 
knowledge  of  the  ground  commander  as  to  his  position  and  orientation  within  the  com¬ 
bat  arena.  The  units  involved  in  a  joint  maneuver  must  coordinate  their  positions  and 
orientations  with  each  other  in  order  to  ensure  maximum  cooperation  and  the  successful 
prosecution  of  the  military  campaign.  Recent  history  dictates  that  the  knowledge  of  a 
tactical  military  unit  position  has  rested  in  the  individual  skill  of  members  of  the  unit 
with  a  map  and  lensatic  compass,  from  which  a  position  could  be  determined.  Orien¬ 
tation  of  the  unit  could  be  controlled  through  the  accurate  association  of  surrounding 
prominent  terrain  features  to  the  map.  Once  location  and  orientation  have  been  estab¬ 
lished,  they  must  be  transmitted  to  higher  headquarters  in  order  to  ensure  the  proper 
adjacent  unit  coordination.  This  knowledge,  along  with  tactical  intelligence  information 
about  the  position  and  orientation  of  local  enemy  forces  allows  the  ground  commander, 
through  his  staff,  to  command  and  coordinate  support  operations.  These  operations 
would  include  indirect  fire  support,  dose-air  support,  deep-air  support,  and  logistics  and 
communication  support. 

The  Position  Location  Reporting  System  (PLRS)  was  created  by  the  Hughes  Cor¬ 
poration  of  Los  Angeles  for  the  Marine  Corps  and  Army  to  ensure,  or  at  least  improve, 
command  knowledge  of  the  positions  and  orientations  of  friendly  tactical  elements 
within  the  combat  arena  [Ref.  1].  This  system,  however,  does  not  track  enemy  forces 
forward  of  the  Forward  Edge  of  the  Battle  Area  (FEBA).  This  can  be  overcome  by  the 
use  of  an  extended  Kalman  filter  algorithm  designed  as  the  tracking  element  with  inputs 
from  a  fused  sensor  array.  The  sensors  employed  would  be  restricted  to  four  different 
types  of  observables:  target  bearings,  target  ranges,  both  target  bearings  and  target 
ranges,  or  a  Doppler-shifted  frequency  signal  accompanied  by  the  bearing  to  that  signal. 
The  Kalman  filter  algorithm  would  take  the  sensor  inputs  and  through  a  conversion 
process,  show  observed  and  estimated  target  tracks  accompanied  by  a  predicted  track 
based  on  the  last  estimated  position  of  the  target.  An  addition  to  the  Kalman  filter  al¬ 
gorithm  could  then  allow  for  the  display  of  ellipsoids  of  constant  probability  (error 
ellipsoids)  about  the  estimated  target  positions  to  show  an  area  in  which  the  target  is 
located  to  a  definite  probability. 
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The  operation  ofPLRS  will  be  discussed,  at  least  partially,  in  the  next  chapter.  The 
accumulated  knowledge  of  the  PLRS  observables  (position  and  location  of  friendly 
units)  and  the  observables  of  the  algorithms  developed  in  this  thesis  could  contribute 
greatly  to  the  knowledge  that  a  potential  ground  tactical  commander  could  gain  about 
his  situation  within  the  combat  arena  and  serve  to  aid  in  the  tactical  decision  making 
process.  The  knowledge  of  friendly  and  enemy  forces  within  a  closed  combat  arena  is 
essential  to  the  successful  prosecution  of  military  objectives.  This  axiom  is  best  summed 
up  by  the  Chinese  philosopher  general,  Sun  Tzu  [Ref.  2]  who  said 

If  you  know  the  enemy  and  know  yourself,  you  need  not  fear  the  result  of  a  hundred 
battles.  If  you  know  yourself  but  not  the  enemy,  for  every  victory  gained  you  will 
also  suffer  a  defeat.  If  you  know  neither  the  enemy  nor  yourself,  you  will  succumb 
in  every  battle  .... 

One  can  easily  see  the  necessity  for  accumulated  combat  maneuver  intelligence. 
Since  PLRS  has  no  ability  to  track  an  enemy  force  which  is  forward  of  the  FEBA  and 
concentrates  solely  on  friendly  units,  a  potential  tracking  algorithm  is  developed  in  an 
attempt  to  augment  the  PLRS  system.  The  algorithm  is  as  previously  described  and 
based  on  multiple  fused  sensors  providing  measured  observables  to  the  tracking  element 
for  processing.  Four  types  of  observables  were  considered:  target  bearings  only,  target 
ranges  only,  both  target  bearings  and  target  ranges,  and  a  Doppler-shifted  frequency 
accompanied  by  the  bearing  to  that  frequency.  These  observables  are  then  fed  to  an  ex¬ 
tended  Kalman  filter  simulation  algorithm  to  produce  the  estimated  and  the  predicted 
tracks. 

The  extended  Kalman  filter  assumes  the  existence  of  nonlinearities  between  the  ob¬ 
servables  and  the  system  states.  The  simulations  conducted  restrict  movement  of  the 
target  to  the  XV  plane  and  the  system  states  are  derived  from  these  dimensions.  Non- 
linearities  to  the  filter  algorithms  occur  in  all  of  the  observables  considered  relative  to 
the  derived  system  states.  An  error  ellipsoid  algorithm  is  then  employed  in  order  to  fur¬ 
ther  isolate  the  target  and  to  attempt  to  confirm  its  probable  location.  The  algorithms 
are  then  combined  using  a  personal  computer-based,  user-friendly  driver  routine  in  order 
to  acquire  the  necessary  inputs.  The  total  simulation  program  is  then  analyzed  for  per¬ 
formance  and  an  assessment  of  its  tracking  capabilities  provided. 
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II.  POSITION  LOCATION  REPORTING  SYSTEM 

The  PLRS  system  was  developed  for  many  different  reasons  -  among  them  was  the 
ability  to  protect  friendly  tactical  elements  from  fire  from  friendly  sources  during  combat 
situations  and  during  peacetime  training  exercises.  Accidents  occur  every  year  at  the 
Marine  Corps  Air  Ground  Combat  Center  (MCAGCC),  Twentynine  Palms,  California, 
which  are  a  direct  result  of  a  lack  of  knowledge  as  to  subordinate  unit  positions  by 
command  elements  of  friendly  units.  This  is  caused  by  untimely  updating  over  command 
communication  nets.  The  Combined  Arms  Exercise  (CAX)  and  the  Regimental  Firing 
Exercise  (FEX)  are  commonly  used  to  combine  actual  movement  of  troops  with  live  fire 
in  order  to  impress  upon  commanders  the  importance  of  proper  coordination  and  to 
increase  realism  in  combat  training.  All  of  the  movement  corridors  at  MCAGCC  are 
considered  as  live  fire  areas  and,  therefore,  the  positions  of  all  maneuver  elements  within 
them  is  considered  essential  to  both  personnel  safety  and  to  mission  accomplishment. 

The  system  consists  of  a  containerized  master  unit  composed  of  generalized  com¬ 
puter,  communications,  and  command  electronics.  This  unit  is  designated  as  the  Master 
Unit  (MU).  It  is  responsible  for  processing  all  information  inputs  into  the  system  and 
for  the  display  of  unit  positions,  coordination  measures,  and  control  information  visu¬ 
ally.  Locally-placed  user  units  are  man  or  machine  portable  and  are  designated  as  User 
Units  (UU).  The  MU  sends  a  radio  message  to  the  UU  which  then  computes  a  time  of 
arrival  (TOA)  for  the  message  and  transmits  this  information  back  to  the  MU.  The  MU 
then  uses  the  arrival  time  information  from  the  UU  to  compute  a  range  to  the  UU.  The 
UU  also  transmits  the  local  barometric  pressure  to  the  MU.  The  \1U  then  uses  the 
range,  bearing,  and  pressure  information  to  compute  a  three-dimensional  position  of  the 
UU.  Since  the  MU  will  not  always  have  line-of-sight  radio  contact  with  all  of  the  local 
user  units,  the  system  relies  on  the  user  units  to  determine  the  distances  between  them¬ 
selves  and  other  locally-placed  user  units  by  computation  of  a  TOA  for  the  transmissions 
of  the  other  units  and  to  report  this  range  information  to  the  MU.  Reports  are  made 
to  the  MU  through  a  multi-level  relay  technique  using  other  user  units  as  necessary*. 

Within  PLRS,  the  location  of  each  unit  is  established  through  a  process  called 
trilateration.  This  involves  taking  the  ranges  from  three  other  units  with  previously  es¬ 
tablished  positions  and  using  those  ranges  to  calculate  the  position  of  the  unlocated  unit. 
To  accomplish  this  the  MU  uses  four  simplified  discrete  Kalman  filters.  These  filters  are 
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called  the  Central  Logic  Oscillator  Control  (CLOCj  filter  to  process  clock  offset  and  drift 
rate,  the  Mean  Sea  Level  (MSL)  filter  to  process  an  offset  calibration  for  the  barometric 
pressure  measurements  reported  to  it,  the  Track  Review  and  Correction  Estimation 
(TRACE)  filter  to  enter  and  partially  update  each  UU  position  and  \elocity  estimate, 
and  an  altitude  filter  to  process  the  barometric  pressure  data  in  order  to  establish  and 
aid  in  tracking  the  vertical  positions  of  the  local  user  units. 

Tracking  of  the  user  units  is  accomplished  by  the  ML'  using  an  internal  coordinate 
system.  The  internal  coordinate  system  serves  as  the  basis  for  all  coordinate  transfor¬ 
mation  processing.  This  system  is  a  transverse  mercator  projection  with  its  central 
meridian  at  the  longitude  of  system  center.  The  system  essentially  takes  the  curved  sur¬ 
face  or  the  earth  and  projects  it  stereographically  onto  a  flat  plane.  This  is  then  called 
the  Local  Transverse  Mercator  (LTM)  system.  The  LTM  system  produces  a  slight 
amount  of  distortion  for  long  range  measurements  at  distances  far  from  system  center. 
[Ref.  3] 

To  use  the  LTM  system,  the  TOA  measurements  must  be  converted  to  LTM  ranges 
by  PLRS  in  real  time.  This  conversion  is  accomplished  in  four  steps  by  the  ML  and  is 
illustrated  in  Figure  1  on  page  5.  The  four  steps  are  listed  as  follows: 

1.  The  conversion  of  the  TOA  measurement  to  a  true  range  measurement. 

2.  The  conceptual  movement  of  the  units  to  an  average  altitude  of  zero. 

3.  The  projection  of  the  earth  onto  a  fiat  plane. 

4.  The  movement  of  the  units  back  to  their  original  altitudes. 

The  use  of  PLRS  greatly  improves  command  and  control  efforts  at  the 
regimental  brigade  level  and  above.  By  provision  of  improved  manuever  unit  movement 
updates  and  an  increase  in  communications  speed,  the  PLRS  concept  significantly  im¬ 
proves  fire  support  coordination  and  logistical  support  efforts.  The  improvements  gained 
decrease  personnel  safety  risks  while  allowing  for  realism  in  peacetime  training,  and  in¬ 
creased  unit  effectiveness  in  combat. 
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C.  INTERNAL  ILTM)  RANGE 


I  ~  INTERNAL  (LTM)  RANGE  I 


MU  Conversion  of  TOA  Data  to  LTM  Range:  From  [Ref.  3] 


III.  MULTIPLE  GEOMETRY  TARGET  TRACKING  ALGORITHM 

In  order  to  study  the  problem  by  simulation,  three  different  types  of  simulation 
models  had  to  be  developed.  The  sensors,  when  actually  employed  in  the  field,  would 
track  a  real  moving  target  across  the  ground.  Since  a  real  target  does  not  exist,  for  the 
purposes  of  this  simulation,  a  model  had  to  be  developed.  The  sensor  system  must  also 
be  modeled  so  that  the  observables  developed  from  it  can  be  fed  to  the  extended  Kalman 
filter  algorithm  for  processing.  The  extended  Kalman  filter  must  be  modeled  in  a  com¬ 
puter  algorithm  so  as  to  demonstrate  its  tracking  capability.  The  simulations  must  then 
be  iterated  in  order  to  demonstrate  target  movement  and  subsequent  updating  of  the 
estimated  positions  and  velocities  of  the  modeled  target. 

A.  TARGET  MODEL 

The  motion  of  the  target  is  restricted  to  the  XV  plane.  The  simulation  model  uses 
the  basic  equation  of  motion  from  kinematics 

s  =  s0  +  s0T  + -j  asT2  (3.1) 

This  relation  is  applied  in  both  the  X  and  Y  directions  and  over  the  total  observation 
time 


Ttoial  ~  ^max  x  T  (3.2) 

where  km,t  is  the  number  of  target  track  positions  desired.  Thus,  a  complete  set  of  target 
positions  over  a  defined  time  period  can  be  defined.  These  positions  can  then  serve  as 
the  basis  for  the  sensor  and  Kalman  tracker  algorithms. 

B.  SYSTEM  MODEL 

The  system  modeled  is  that  of  a  ground  target  moving  in  the  XY  plane.  Certain 
restrictions  were  placed  on  the  problem  in  order  to  simplify  the  mathematics.  These  re¬ 
strictions  were: 

•  the  curvature  of  the  earth  is  neglected, 

•  target  and  sensor  movement  are  restricted  to  a  fiat  plane,  and 

•  target  course  and  speed  inputs  are  constant  (i.e.,  step  inputs). 
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The  observed  matrix  developed  from  the  senior  simulation  algorithm  contains  no  noise, 
so  that  Gaussian  noise  can  be  added  to  the  observations  before  entry  into  the  extended 
Kalman  filter  algorithm.  This  supports  realism  in  the  simulation  in  that  the  observation 
measurements  would  contain  Gaussian  noise. 

This  is  a  linear,  time-distance  system  that  can  be  described  with  equations  of  motion 
for  constant  acceleration  in  two  dimensions.  The  state  space  equation  is 

**+t  “  (3  3) 


where 

xt  state  vector  (estimation  parameter) 

<£*  =  state  transition  matrix  (describes  how  the  dynamic  system  states  are  related) 

4^,  =  system  noise  coefficient  matrix 
a,  =  random  forcing  function. 

When  the  above  conditions  and  Equation  (3.3)  are  incorporated,  and  the  observable 
system  is  target  bearings  only,  target  ranges  only,  or  both  target  bearings  and  target 
ranges,  the  state  vector  is 


(3.4| 


When  the  set  of  observables  is  the  Doppler-shifted  frequency  measurement  and  the  as¬ 
sociated  bearing  to  that  frequency,  then  the  state  vector  is 


(3.5) 


where  /  is  the  target  transmission  rest  frequency. 

When  the  following  model  equations  are  used  and  the  aforementioned  problem 
conditions  incorporated  into  them,  the  system  state  equation  can  be  expanded  in  matrix 
form.  The  model  equations  are 
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=  xk  +  xkT+-jaxkT2 

(3.6) 

■*k+\  =  *k  "b  axkT 

(3.7) 

=yk+yJ + y  ayk  t2 

(3.8) 

y'k- t-t  —  yk  °yk^ 

(3.9) 

JSs 

J*- 

» 

II 

(3.10) 

Thus  for  the  systems  where  the  state  vector  is  described  by  Equation  (3.4).  the  system 
state  equation  can  be  expressed  as 


X 

"l 

T 

0 

o' 

-  - 

X 

I 

0 

X 

<) 

1 

0 

0 

X 

T 

0 

y 

k+\  - 

0 

0 

1 

T 

y 

k  + 

0 

T212 

_y_ 

0 

0 

0 

1 

0 

T 

For  the  systems  where  the  state  vector  is  defined  by  Equation  (3.5).  the  system  state 
equation  can  be  expressed  as 


X 

1 

T 

0 

0 

o" 

X 

T 

r/2 

0 

0 

0 

A 

X 

0 

1 

0 

0 

0 

X 

0 

T 

0 

0 

0 

fi 

y 

k+ 1  ~ 

0 

0 

I 

T 

0 

y 

k  + 

0 

0 

T 

T21 2 

0 

A 

k 

(3.12) 

y 

0 

0 

0 

1 

0 

y 

0 

0 

0 

T 

0 

A 

fo 

0 

0 

0 

0 

1 

A 

0 

0 

0 

0 

T 

fs 

where  f  through  fs  are  the  random  forcing  functions  included  to  account  for  random 
changes  in  speed,  direction,  or  transmission  frequency  which  can  occur  for  a  moving 
target.  Each  of  these  random  forcing  functions  must  be  considered  independently  in  or¬ 
der  to  ascertain  their  effect.  The  forcing  functions  can  be  expressed  by 

f  =fi(y9r>  yv,> k)  (3.13) 

fi  'V  (3.14) 
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A  =h (Vg;  >'v  k) 
A  ~A(y0'’  Vyr> 


(3.15) 


(3.16) 

fs  =/s(y/e)  ’  (3-17) 

where  (for  heading).  v,(  (for  speed),  and  y/o  (for  the  transmission  rest  frequency)  are 
considered  as  random  changes  to  the  target.  These  changes  are  assumed  to  be  inde¬ 
pendent,  zero  mean,  and  piecewise  constant  rates  of  change.  They  have  variances  defined 
as 


\  -  4(’/v,)2] 

(3.1S) 

7l,  =  £[(>'0/] 

(3.19) 

(3-20) 

The  system  noise  process  for  the  target  tracking  and  prediction  problem  is  a  function 
of  the  system  noise  coefficient  matrix  A*,  and  the  random  forcing  function  a „  as  defined 
for  each  of  the  state  vectors.  For  the  state  vector  of  Equation  (3.4)  this  is  simply  the 
target  acceleration  vector.  For  the  state  vector  of  Equation  (3.5)  a*  is  defined  by 

/>" 

A 

ak=  A  ■  (3-21) 

/a 

As 

The  state  space  representation  for  the  Doppler-shifted  frequency  observable  was  devel¬ 
oped  by  Mitschang  (Ref.  4]. 

C.  MEASUREMENT  MODEL 

The  problem,  as  studied,  considered  four  types  of  observables:  bearings  to  the  target 
only,  ranges  to  the  target  only,  both  bearings  and  ranges  to  the  target,  and  the 
Doppler-shifted  frequency  accompanied  by  a  bearing  measurement  to  that  frequency. 
With  recall  of  the  state  vectors  as  defined  by  Equations  (3.4)  and  (3.5),  the  measurement 
model  for  this  problem  can  be  developed.  The  measurement  equation  is 
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*k  Hk&k  "b 


(3.22) 


where 

zk  —  the  set  of  measurements 

i£,  =  the  observ'ation  matrix  (noiseless) 

=  the  state  vector 
12,  =  associated  measurement  noise. 

1.  Bearings  Only  Problem 

In  this  tracking  problem,  the  measurements  are  lines  of  bearing  as  received  by 
sensors  located  on  separate  prominent  terrain  features.  The  geometry  of  the  problem  is 
shown  in  Figure  2  on  page  11.  The  problem  geometry  shows  that  the  relationship  of 
the  measurements  to  the  system  states  is  nonlinear.  For  this  case  the  measurement 
equation  becomes 


6nk  =  tan 


~  x,.k) 

O'/*  ~~  }nk) 


°k 


(3.23) 


where 

0,4  =  lines  of  bearing  from  sensor  n  at  time  k 
x.,v..  =  the  X.Y  coordinates  of  the  target  at  time  k 
jcn*y„4  =  the  X.Y  coordinates  of  sensor  n  at  time  k 
u*  =  the  bearing  measurement  noise. 

2.  Ranges  Only  Problem 

In  this  tracking  problem,  the  measurements  are  straight-line  distances  to  the 
target  gained  by  ranging  sensors  located  on  separate  prominent  terrain  features.  The 
problem  geometry  is  shown  in  Figure  3  on  page  12.  This  geometry,  as  in  the  last  case, 
shows  a  nonlinear  relationship  between  the  measurements  and  the  system  states.  The 
measurement  equation  is 

rnk  ~  \  xtk  ~~  *nk  "b Tfk  —  }'nk  vk  (3-24) 

where  =  range  distances  from  sensor  n  at  time  k. 
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Target 


2 


Figure  2.  Bearings  Only  Geometry:  Measurement  Inputs  to  the  Kalman  Filter 
are  Restricted  to  Target  Bearings. 
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Figure  3.  Ranges  Only  Geometry:  Measurement  Inputs  to  the  Kalman  Filter  are 
Restricted  to  Target  Ranges. 
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3.  Bearings  and  Ranges  Problem 

The  tracking  problem  now  becomes  a  combination  of  the  first  two  problems. 
The  measurements  are  defined  as  in  Equation  (3.23)  for  target  bearings  and  Equation 
(3.24)  for  target  ranges.  The  geometry  for  this  problem  is  shown  in  Figure  4  on  page 
14.  The  observation  matrix  for  this  problem  is  now  defined  as 

V 

«  A.  (3-25) 

Vi 

r2 


4.  Doppler-Shifted  Frequency  Problem 

In  this  tracking  problem,  the  observables  are  a  Doppler-shifted  frequency  ac¬ 
companied  by  a  bearing  measurement  to  that  frequency.  The  problem  geometry  as 
shown  in  Figure  5  on  page  15.  along  with  knowledge  of  the  Doppler  equation  [Ref.  5], 
shows  that  the  relationship  between  the  measurements  and  the  system  states  is  again 
nonlinear.  The  measurement  equation  is 


zk 


Xlk  ~  - Xnk 

}'tk  ~  y'nk 


— 


.  m)vP 

■xikx;':  +  ytrS'sk 
\xik  ^  y>k 


+ 


(3.26) 


where 

v,  =  the  velocity  of  wave  propagation  (speed  of  light) 
v,*  =  bearing  measurement  noise 
Vp,  =  frequency  measurement  noise. 

This  measurement  equation  is  based  on  a  single  sensor  which  is  stationary  and  located 
at  the  origin. 

The  observables  in  all  four  measurement  cases  have  been  shown  to  be  nonlinear 
in  their  relationship  to  the  system  states.  Processing  by  the  extended  Kalman  filter  is 
thus  required  for  target  tracking  and  prediction.  The  operation  of  the  extended  Kalman 
filter  will  be  explained  in  the  next  chapter. 
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rigure  4.  Bearings  and  Ranges  Geometry:  Measurement  Inputs  to  the  Kalman 
Filter  are  Restricted  to  Target  Bearings  and  Target  Ranges. 


Figure  5.  Doppler  Frequency  Geometry:  Measurement  Inputs  to  the  Kalman 
Filter  are  Restricted  to  a  Doppler  Shifted  Frequency  and  the  Associated 
Frequency  Bearing. 
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IV.  KALMAN  FILTER  THEORY 

The  process  of  estimation  of  the  state  vector  at  the  current  time,  with  reference  to 
all  previous  measurements,  is  referred  to  as  filtering.  An  optimal  filter  optimizes  a  spe¬ 
cific  performance  measurement  which  is  used  to  approximate  the  quality  of  the  estimate. 
The  Kalman  filter  is  a  linear  optimal  filter  which  minimizes  the  mean  square  estimation 
error  between  the  actual  output  and  the  desired  output.  The  filter,  in  actuality,  is  a  re¬ 
cursive  algorithm  for  the  optimal  processing  of  discrete  measurements  or  observations 
[Ref.  6].  The  filter  requires  an  a  priori  knowledge  of  the  state  estimate  and  its  error 
covariance  as  well  as  the  current  observation.  System  equations  for  the  Kalman  filter 
are 


*k+i  =  (-4-1) 

and 

zk  =  +  12*  (4-) 

A.  THE  EXTENDED  KALMAN  FILTER 

From  the  measurement  equations  in  the  last  chapter  for  each  of  the  observable 
cases,  one  can  see  that  there  is  a  nonlinear  relationship  between  the  measurements  used 
for  target  tracking  and  the  system  state  variables.  The  adaptation  of  the  Kalman  filter 
to  a  nonlinear  application  is  termed  as  the  extended  Kalman  filter.  A  general  discussion 
of  the  of  this  type  of  filter  algorithm  follows. 

Given  system  and  measurement  equations  of  the  form 

£k  *)  +  iUk,  k) wk  (4.3) 

ik  =  hUk,  k)  +  vk  (-  4) 

where 

/,  g,  h  are  nonlinear  functions  of  the  state  vector  £  , 
ik*  is  the  plant  excitation  noise, 
v*  is  the  measurement  noise. 
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The  plant  excitation  noise  and  the  measurement  noise  are  assumed  as  uncorrelated,  zero 
mean,  and  white.  That  is 


ik*  =  -V(0,  Q ), 

(4.5) 

I*  -  A'(0,  R), 

(4.6) 

and 

flik'ir,  iky  ]  =  QlkJ 

(4.7) 

-k'  -j  ]  =  BIkj 

(4.S) 

where  5kJ  is  the  Kronecker  delta  function  such  that 

1! 

•4* 

II 

-*<r 

(4.9) 

% 

•4* 

o 

II 

(4.10) 

To  apply  the  linear  filter  Equations  (4.1)  and  (4.2),  an  expansion  of  Equations  (4.3) 

and  (4.4)  is  conducted  about  the  best  estimate  of  the  state  at  the 

current  time.  This  ex¬ 

pansion  is  accomplished  with  a  Taylor  series  and  only  the  first 
With  the  expansion.  Equation  (4.3  >  now  yields 

order  terms  are  kept. 

£k +i  =  £***  +  &k"k 

(4.11) 

where 

cf 

C£k  *>  =  <** 

(4.12) 

Equation  (4.4)  now  yields 

ik  ~  Hk*k  +  %k 

(4.13) 

where 

u  _  Ik. 

H-k  '  = 

C£k 

(4.14) 

17 


The  variable  &  is  the  estimated  value  of  the  state  after  the  k"'  measurement  and  the 
variable  xik  is  the  predicted  state  value  before  the  k'h  measurement.  These  can  be  speci¬ 
fied  more  completely  by 


£Lk=A£k-i’k-  1)-  (41~) 

The  following  set  of  definitions  can  then  be  used  to  derive  the  linear  Kalman  filter 
equations  used  in  the  simulation.  A  state  error  vector  can  be  defined  as 

£k  =  £k-*k-  (416) 


A  predicted  state  error  vector  can  then  be  defined  by 

£k  =  £Lk-£k-  (4-17) 

Equations  (-1.16)  and  (4.17)  allow  the  definition  or  *’  covariance  of  state  error  matrix 
P .  by 

(-US) 


and  the  definition  of  the  predicted  covariance  of  state  error  matrix  E\  by 

ELk  =  E[?k2r\-  (4.  EM 

I'he  state  excitation  matrix  Ok  ,  as  seen  in  Equation  (4.7),  is  defined  by 

Qk  —  i4--'-’) 

The  measurement  noise  covariance  matrix  ,  as  seen  in  Equation  (4.8).  is  defined  by 

(4-21) 

The  definfions  cited  above  provide  the  basis  for  the  construct  for  the  Kalman  filter  al¬ 
gorithm  specified  by  the  set  of  equations  below  [Ref.  6] 

Zk+^&Ekil+Qk  (4-22) 

Qk  =  E'  kURttkE'  kUl  +  RkT]  (4-23) 

Ek  =  U-QkHk-]Kk  (4-2-D 
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X-k  1  ’  k) 

(4.25) 

zLk  =  h{xLk>  k ) 

(4.26) 

£k  =  £-k  +  G-khk  ~  21*3 

(4.27) 

where 

I  =  the  identity  matrix 

<2k  =  the  Kalman  gain  matrix. 

The  Q  matrix  allows  for  target  maneuvering  and  also  accounts  for  filter  model  in¬ 
accuracies.  This  greatly  decreases  the  discrepancies  which  would  arise  between  the  sys¬ 
tem  characterization,  Equations  (4.1)  and  (4.2),  and  the  true  action  of  the  system 
physically.  As  the  filter  algorithm  reaches  steady-state  conditions,  the  Q  matrix  also 
prevents  the  Kalman  gain  matrix.  <2k  ,  from  approaching  zero  by  ensuring  an  amount 
of  uncertainty  in  the  predicted  covariance  of  the  state  error  matrix  £!_k 

B.  LINEARIZATION  OF  THE  OBSERVATION  MATRIX 

For  purposes  of  brevity,  the  H  matrix  derivations  will  be  done  simultaneously  for 
the  bearings  only,  ranges  only,  and  the  bearings  and  ranges  observable  cases.  These  are 
slightly  different,  however  they  are  based  upon  the  same  mathematical  premise. 
Equations  (4.4),  (4.13),  and  (4.14)  establish  the  basis  for  the  linearization  of  the  obser¬ 
vation  matrix  H.  These  equations  state  that  the  H  matrix  is  a  linearization  of  the  non¬ 
linear  function  h  and  is  achieved  by  taking  the  partial  derivative  of  h  with  respect  to  the 
predicted  state. 

1,  Target-Bearing  Measurement  Observables  Only 

The  h  function  using  bearing  measurements  only  can  be  deduced  from  Equation 
(3.23)  as 


h{&k,  k)  =  tan' 


[ 


x,k~xnk  ~ 
y'lk  ~  }nk 


Applying  the  above  linearization  method, 


ffc- 


tan 


— ! 


[  (x,k  -  xnk ) 
[_  (df k  ~  }nk ) 


C£k 


(4.28) 


(4.29) 
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where  the  range  estimate  squared  R2ni  is 


R-nk  )Xt(k,  k-\)  xnk )  CKr(A. Jfe — I )  }'nk)  ' 


(4.39) 


The  second  row  of  the  matrix  is  completed  in  a  similar  manner. 

2.  Target-Range  Measurement  Observables  Only 

The  target-range-measurement-only  h  function  can  be  deduced  from  Equation 
(3.24)  as 


hUk.  k)  =  v(.rf*  -  xnkf  +  Ov*  -y'nkf 


(4.40) 


Applying  the  linearization  method  again  yields 


(xik  xnk )  0 ik  }nk)  J 


(4.41) 


where  again,  x,  is  recalled  from  Equation  (3.4).  Simplification  of  Equation  (4.36)  yields 


_  pi 

'  _hi 


hn  /t, 3  /i, 


21  ^22  *23  *2 a 


where 


?y(Xlk  ~  xnkf  +  <S,k  ~  }'nk)2  ]  ytk  -  ynk 


h n2 


C £  \  (xtk  xnk )  9-  (Vj/j  } n )  J 


(4.44) 


= 


5 [ v(*f*  -  xnk)2  +  OrA  - ynkf  ]  -  (Xlk  -  Xn 


(4.45) 


s\  y(x,k  -  xnk )2  +  iy,k  -ynk)1 1 

h  =  - 7- - -  =  o. 

°}'lk 


(4.46) 


Thus  from  Equations  (4.43)  through  (4.46) 
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(4.47) 


x,k  ~  xnk 

*11  - - 77 - 

hn  =  0  (4.48) 

^3=-C-^— ’  (4.49) 

tit 

*14  =  0  (4.50) 

and  as  before,  the  second  row  can  be  computed  in  a  similar  manner. 

3.  Target-Bearing  and  Range  Measurement  Observables 

The  h  functions  for  the  case  where  the  observables  are  both  the  target  bearings 
and  the  target  ranges  are  the  same  as  those  derived  in  both  of  the  previous  cases.  These 
cases,  however,  are  now  combined.  The  corresponding  H  matrix  is 


2ik  = 


*u 

*12  *13  *14 

*21 

*22  *23  *24 

*31 

*32  *33  *34 

*4, 

*42  *43  *44 

(4.51) 


where  the  first  and  third  row  of  the  matrix  are  the  first  and  second  rows  of  the  matrix 
defined  in  Equation  (4.30)  replaced  in  a  one-for-one  correspondence.  The  second  and 
fourth  rows  of  the  matrix  are  the  first  and  second  rows  of  the  matrix  defined  in  Equation 
(4.42)  replaced  one-for-one. 

4.  Doppler-Shifted  Frequency  Observable 

The  h  function  for  the  Doppler-shifted  frequency  observable  and  the  associated 
bearing  is  somewhat  more  detailed.  Using  the  state  vector  defined  by  Equation  (3.5)  and 
the  measurements  obtained  from  Equation  (3.26),  one  can  deduce  the  following  shifted 
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Doppler  h  function 


h(zk,  k)  = 


X,k  ~  xk 

y,k  -y'k 


MkK 


r-  -I 

l'  + 

xrkx:k  4"  }  [i-}  :k 

p 

,  2  ,  2 

L  \  xtk  "*■  }ik  J 

(4.52) 


Application  of  the  linearization  process  yields 


L/,= 


h i  j  h\2  7  ^14  hlS 

k2]  ^’3 


where  using  the  notation  of  Equation  (4.15) 

rO: 

/!;  ,  =  -T—  =  — - — 

-v  Ik  +  y  u 


h 


i: 


rd k 

c~x;k 


hu  =  ~ - = 


.V 


Oik 


x  'tk  +  y  Ik 


/l,4  = 


(T..1 


=  0 


C&j, 

h]5  =  Jk=° 


(4.55) 


1 4. 54) 


(4.55) 


(4.56) 


(4.51 


(4.5S) 


cfk  _  ~f'ly'rkL>:’rkx'rk  ~  x' iQ'' tkl 


(4.59) 


h22  = 


Vk 


-rU* 


c.x 


ik 


f'ok'pl^ik  + 


*2  •'2'i 


(4.60) 
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(4.61) 


*fk  -f'l x' ,klx' ik)’’ ,k  ~ 


/t23  — 


O'* 


/'o*v>c(^L+yL)3/2] 


h2a  — 


% 


/Wri +/*>"'] 


..2  xW2i 


*25  = 


*/0  /' 


Ok 


(4.62) 

(4.63) 


Having  linearized  the  observation  matrices  for  all  of  the  observable  cases,  the  linear 
Kalman  filter  equations  can  be  utilized. 


C.  ACQUIRED  NOISE  PROCESSES  (R  AND  Q  MATRICES) 

Calculation  of  the  covariance  of  state  error  matrix,  £t,  and  the  Kalman  gain  matrix 
Q,.  requires  the  specification  of  covariance  matrices  for  the  associated  uncorrelated  noise 
processes  a,  and  v,  as  used  in  this  study.  The  covariance  matrix  for  the  measurement 
noise  process  v*  is  found  in  Equation  (4.8).  .£*  is  defined  as  the  state  measurement  noise 
covariance  matrix.  This  matrix  is  based  on  the  accuracy  of  the  sensors  employed  and 
accounts  for  unknown  disturbances  in  the  plant  model. 

The  state  excitation  matrix  Q„  represents  the  system  noise  process  and  is  a  function 
of  the  system  noise  coefficient  matrix  A*  and  the  random  forcing  function  .  Thus 


2*-IA2j4*] 


(4.64) 


where  Q\  ,  from  Equation  (4.20),  is  defined  as 


Qik  =  £[W]  = 


E{alk)  E(aykaxk) 
E{axjiOyii)  E{ayk) 


(4.65) 


This  matrix  is  valid  for  the  bearings  and  ranges  observables  and  allows  for  any  random 
target  maneuvers  and  any  inaccuracies  in  the  system  model. 

The  derivation  of  the  Q  matrix  for  the  Doppler  observable  is  slightly  more  complex 
however,  and  is  based  on  the  random  disturbances  /  through/  as  defined  by  Equation 
(3.21).  Recall  that  y-t  ,  y,t  ,  and  y/o  were  the  random  changes  to  the  target  and  were  as¬ 
sumed  as  independent,  zero  mean,  and  piecewise  continuous.  The  variances  for  these 
changes  can  be  defined  as 


24 


-  £t>y 

(4.66) 

-  T'4,] 

(4.67) 

11 

rn 

i — i 

i _ i 

(4.68) 

.  The  standard  deviations  a>r  a-t,  and  a  specify  typical  maneuvering  parameters  for  the 
target.  The  Q  matrix  is  found  by  letting 
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where  the  states  are  evaluated  at  the  current  state  estimates  i*.  Substitution  of  the  above 
expressions  into  the  Q  matrix  yields 
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This  analysis  was  done  by  Mitschang  and  is  found  in  detailed  form  in  Reference  4. 


D.  PROBLEM  PARAMETERS 

The  following  parameters  were  used  in  this  analysis: 
<7,r  =  6.283  rad/hr 
a ,t  -  0.01852  km/hr 
=  0.01096  (rad/min)2 
a =0.0001852  (km/min2):. 

For  the  Doppler  observable,  the  parameters  were: 
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=  0.00017452  rad/sec 
a„'  =  0.01852  km/scc 
=0.0005  Hz/sec. 

With  the  (p  matrix  defined  by  the  system  considered  as  in  Chapter  III,  and  the  Q,  R,  and 
H  matrices  defined  as  above,  the  Kalman  filter  Equations  (4.22)  through  (4.27)  can  be 
employed  and  the  simulations  conducted. 
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V.  ERROR  ELLIPSOIDS 


The  position  of  each  unit  within  the  PLRS  system  is  estimated  with  a  certain  finite 
degree  of  uncertainty  about  the  estimate.  The  associated  uncertainty  expressed  in  the 
covariance  of  error  matrix  Pk.  This  represents  the  sigma  squared  error  deviation  about 
the  estimate.  The  position  diagonal  terms,  Pn  and  Pn,  represent  the  variances  of  the  es¬ 
timate  in  the  X  and  Y  directions  respectively.  The  off-diagonal  terms  of  each  represent 
covariances,  the  degree  of  coordinate  coupling,  and  the  orientation  of  the  uncertainty  in 
the  XV  plane. 

The  errors  are  normally  distributed  and  there  exists  a  rotated  coordinate  system  in 
which  the  orthogonal  position  components  are  uncorrelated.  This  is  identical  to  taking 
the  exponent  of  the  joint  probability  density  function  and  performing  a  coordinate 
transformation  to  eliminate  the  cross  terms. 

The  exponent  for  Gaussian  random  variables  is 


2  r. 


.rv 


+  ■ 


(5.1) 


where  r,y  is  the  cross  correlation  of  X  and  Y.  When  this  expression  is  set  equal  to  a 
constant,  the  geometric  interpretation  is  that  of  an  ellipse  which  specifies  a  constant 
probability  of  target  location  about  the  estimate.  The  major  and  minor  axis  of  the  ellipse 
are  oriented  in  the  XY  coordinate  system.  In  this  system  a  cross  correlation  between  X 
and  Y  exists.  To  eliminate  this  cross  correlation,  a  coordinate  transformation  is  applied. 
The  positional  component  X  is  transformed  to  a  new  component  X'  and  the  positional 
component  Y  is  transformed  to  a  new  component  Y'  by 

x'  =  x  cos  6  +>'  sin  0  (5.2) 


and 


x'  =y  cos  0  —  x  sin  0 


(5.3) 


where 


0  =  y  tan  ’[ 
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This  specifies  the  variances  in  the  new  coordinate  system  (XT')  as 


2 

<v  = 


7X  +  0‘V 
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COY(jcy) 

sin  20 


(5.5) 


and 


2  i_  2 

GX  +  Oy 


COV(.YV) 

sin  20 


(5.6) 


The  relation  used  to  plot  the  resulting  ellipsoid,  which  defines  a  constant  probability  of 
target  location  about  the  latest  Kalman  estimate,  results  from  the  transformation.  The 
relation  is  based  on  Equation  (5.1)  and  is 

v'2  v'2 

JT'  +  ^T=K.  (5.7) 

axT  °y • 


where  K  defines  the  pr  ability  of  target  location. 

The  error  ell’psoid  routine  employed  in  this  study  is  based  on  the  above  premise  and 
plots  ellipsc  as  respresenting  a  constant  probability  of  target  location  about  the  latest 
Kalman  estimate  and  uses  that  estimate  as  the  ellipsoid  center.  The  program  was  written 
by  Captain  Stephen  L.  Spehn  USMC  in  collaboration  with  another  project.  This  pro¬ 
gram  plots  a  single  standard  of  deviation  (in  the  orthogonal  coordinates)  ellipsoid  with 
a  sixty  percent  probability  of  target  location  within  the  ellipsoid.  The  major  and  minor 
axis  of  the  ellipse  may  also  be  plotted  in  terms  of  the  Mahalanobis  distance  or  the  dis¬ 
tance  in  standards  of  deviation  {Ref.  7],  The  ellipsoids  are  increased  in  size  by  factors  of 
as  much  as  sixteen  for  ease  of  view. 
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VI.  PROGRAM  RESULTS 

A.  PROGRAM  LOGIC 

The  tracking  problem  solution  integrates  all  four  types  of  geometric  concepts  of 
Chapter  III.  In  order  to  simulate  the  problem  and  to  attempt  a  solution,  each  of  the 
geometric  concepts  required  an  individual  working  subroutine.  The  subroutines  were 
then  integrated  using  a  menu-driven,  user-interactive  driver  file.  The  driver  file  was  de¬ 
signed  to  acquire  target  position  and  movement  information,  tracking  problem  parame¬ 
ters,  and  to  determine  the  type  of  problem  to  be  solved. 

The  program  presented  herein  is  an  attempt  to  solve  the  geometric  problem  of 
tracking  an  arbitrarily  moving  target  in  the  XV  plane.  It  was  written  in  PC-\latlab  in 
order  to  use  a  personal  computer-based  mathematical  algorithm.  The  system,  wher 
augmented  and  improved  with  combinational  logic  and  mapping  data,  could  be  used  by 
a  maneuver  commander  in  the  combat  arena  as  an  additional  tool  in  his  assessment  of 
the  tactical  situation.  The  advantage  that  could  be  gained  in  the  knowledge  of  enemy 
position  and  intent  would  greatly  aid  in  the  tactical  decision-making  process. 

1.  PC-Matlab  Structure 

The  personal  computer-based  Matlab  program  uses  a  unique  program  structure 
that  is  similar  to  Fortran.  The  program,  however,  uses  different  terminology  in  its 
structure  when  employing  driver  routines.  The  difi'erence  in  terminology  is  derived  from 
the  way  the  main  Matlab  driver  programs  are  written.  All  Matlab  execution  files  are 
named  with  a  .m  suffix.  Thus,  all  of  the  file  names  for  this  problem  contain  that  suffix. 
The  driver  file,  marine. m,  calls  the  various  subroutines  as  needed  when  in  operation.  The 
programs  written  for  the  study  of  this  problem  are  included  in  the  Appendix.  They  are 
commented  throughout  in  attempt  to  help  the  reader  to  become  familiar  with  their 
construct  with  as  little  effort  as  possible. 

2.  Subroutine  Structure 

The  program  subroutines  each  isolate  one  geometric  concept  in  an  attempt  to 
simulate  and  solve  the  tracking  problem.  Each  subroutine  is  written  to  simulate  the  tar¬ 
get,  to  simulate  the  observers  (sensors),  and  to  simulate  the  operation  of  the  extended 
Kalman  filter  discussed  in  Chapter  IV.  The  various  steps  are  commented  throughout  in 
an  effort  to  aid  the  reader  in  understanding  the  program  and  its  structure.  The  names 
of  the  variables  used  in  the  programs  of  the  Appendix  correspond  to  those  of  Chapters 
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Ill  and  IV  as  much  as  possible.  It  was  felt  that  this  would  aid  in  the  understanding  of 
its  operation. 

B.  RESULTS 

The  results  of  the  program  discussed  are  presented  graphically  in  order  to  illustrate 
what  the  maneuver  commander  would  see  if  such  a  system  were  to  be  employed  on  the 
battlefield.  Since  the  program  tracks  an  arbitrarily  moving  target  with  arbitrary  observer 
placement,  the  target  tracks  and  the  observer  (sensor)  locations  were  changed  with  each 
geometric  case.  The  cases  will  be  presented  in  the  following  order:  target-bearing  ob¬ 
servables  only  employed,  target-range  observables  only  employed,  both  target-bearing 
and  range  observables  employed,  and  the  Doppler  observable  with  its  associated  bearing 
employed. 
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1.  Target-Bearing  Observables  Only 

Results  of  a  tracking  problem  using  only  the  observed  bearings  from  a  fused- 
sensor  array  are  contained  in  Figure  6  on  page  32  through  Figure  9  on  page  35.  These 
figures  show  outputs  at  various  stages  of  the  program.  Figure  6  on  page  32  shows  the 
actual  track  of  the  target,  the  circles  on  the  diagram,  and  the  noiseless  observations,  the 
dots.  This  graph  allows  for  a  comparison  of  the  bearing  calculations  by  the  program 
against  the  track  as  chosen  in  the  simulation.  This  is  necessary  as  a  check  in  the  opera¬ 
tion  of  the  bearings  only  subroutine.  If  the  comparison  is  successful,  white  noise  is  added 
to  the  bearing  measurements  before  they  are  submitted  to  the  Kalman  algorithm. 
Figure  7  on  page  33  shows  the  actual  and  estimated  target  tracks  as  well  as  the  error 
ellipsoids  around  the  Kalman  estimates.  The  simulation  iterates  and.  when  in  display  on 
the  screen,  one  can  see  the  target  move.  In  this  case,  the  movement  is  from  the  upper 
left  to  the  lower  right.  The  rotation  of  the  error  ellipsiods  to  avoid  coordinate  cross 
correlation  is  clearly  evident.  The  actual  positions  are  again  circles,  the  estimates  are 
crosses,  and  the  error  ellipsoids  are  evident  from  the  plot.  Figure  8  on  page  34  shows  the 
actual,  estimated,  and  predicted  positions  of  the  target.  The  predictions  were  obtained 
by  taking  the  predicted  Kalman  state  estimate  and  the  current  Kalman  state  estimate 
and  performing  a  subtraction  of  the  velocity  values  in  the  X  and  Y  directions  and  di¬ 
viding  by  the  observation  interval.  This  produced  an  estimate  of  the  random  forcing 
function  ak  as  defined  in  Chapter  III.  The  estimate  was  then  used  in  the  linear  Kalman 
system  equation  shown  in  Equation  (4.1)  to  determine  predicted  positions  based  on  a 
constant  acceleration.  This  is  consistent  with  a  second-order  Kalman  analysis  in  each 
direction  where  the  acceleration  is  the  random  forcing  function.  The  predicted  positions 
are  shown  as  stars  in  the  diagram.  Figure  9  on  page  35  shows  what  the  potential  combat 
commander  would  see  should  the  system  be  employed.  The  figure  is  a  combination  of 
the  actual,  estimated,  and  predicted  target  tracks  as  well  as  the  error  ellipsoids  about  the 
Kalman  estimates.  The  figure  also  shows  the  location  of  the  observers,  as  do  the  previ¬ 
ous  figures.  These  are  portrayed  as  an  x  in  the  diagram.  Essentially  the  program  was 
constructed  to  produce  results  in  response  to  questions  a  potential  maneuver 
commander  would  ask  while  engaged  in  combat  operations.  These  questions  are: 

•  Where  is  the  enemy  ? 

•  Where  is  he  going  ? 
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Tigure  6.  Actual  and  Observed  Tracks’  Target-Bearing  Observables  Only.  Ac 
tual  -Track  -  o,  Observed  Track  -  • ,  Sensor  -  x. 
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Figure  7.  Actual  and  Estimated  Tracks:  Target-Bearing  Observables  Only.  Ac¬ 
tual  Track  -  o,  Estimated  Track  -  +  .  The  Plot  Also  Contains  Error 
Ellipsoids  to  Aid  in  Target  Location. 
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2.  Target- Range  Observables  Only 

The  results  of  a  tracking  problem  using  only  target-range  observables  are  shown 
in  Figure  10  on  page  37  through  Figure  13  on  page  40.  These  figures  are  in  the  same 
order  as  the  ones  in  the  bearings  only  case.  The  target  track  and  observer  locations  have 
been  changed  in  order  to  demonstrate  the  versatility  of  the  program.  Figure  10  on  page 
37  shows  the  noiseless  observations  compared  to  the  actual  track  of  the  target  as  chosen 
in  the  simulation.  Again,  if  the  comparison  is  successful,  Gaussian  noise  is  added  before 
filtering.  Figure  11  on  page  38  shows  the  actual  and  estimated  tracks  of  the  target  as 
well  as  the  error  ellipsoids  about  the  Kalman  estimates.  Figure  12  on  page  39  shows  the 
actual,  estimated  and  predicted  target  tracks.  When  the  display  is  on  screen,  one  can 
observe  the  target  movement,  which  in  this  case  would  be  from  left  to  right  across  the 
diagram.  Since  the  observers  are  nearly  colinear  with  the  first  estimate,  the  error  ellipsoid 
shows  a  substantial  bearing  error  while  showing  a  small  range  error.  This  decreases  as 
the  target  moves  away  from  the  observers  because  the  range  directions  are  more  accu¬ 
rately  determined.  Figure  13  on  page  40  is  a  combination  of  the  previous  figures  which 
would  be  used  to  provide  information  about  the  enemy  for  use  in  the  tactical  decision 
making  process. 

3.  Target-Bearing  and  Range  Observables 

The  results  of  a  problem  employing  both  target-bearing  and  range  observables 
.  are  shown  in  Figure  14  on  page  41  through  Figure  17  on  page  44.  The  figures  are  again 
in  the  same  order  as  the  previous  two  cases  to  allow  comparison.  Figure  14  on  page  41 
shows  the  actual  target  track  and  the  noiseless  observations  for  this  case.  If  the  obser¬ 
vations  favorably  compare  with  the  actual  positions,  white  noise  is  added  and  the  noisy 
observations  filtered  by  the  Kalman  algorithm.  Figure  15  on  page  42  shows  the  actual 
and  estimated  tracks  with  the  error  ellipsoids  added.  The  rotation  of  the  ellipsoids  to 
avoid  cross  correlation  of  the  coordinate  variables  is  particularly  evident  in  this  case. 
Figure  16  on  page  43  shows  the  actual,  estimated,  and  predicted  tracks  for  the  target. 
The  target  movement  in  this  case  was  from  the  lower  right  to  the  upper  left.  Figure  17 
on  page  44  is  a  combination  of  the  previous  three  and  would  be  the  result  of  the  program 
when  viewed  in  the  field.  Again,  it  provides  answers  to  relevant  command  questions. 
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Figure  10.  Actual  and  Observed  Tracks:  Target-Range  Observables  Only.  Ac¬ 
tual  Track  -  o,  Observed  Track  -  • ,  Sensor  -  x. 
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Figure  11.  Actual  and  Estimated  Tracks:  Target-Range  Observables  Only.  Ac¬ 
tual  Track  -  o,  Estimated  Track  -  +  .  The  Plot  Also  Contains  Error 
Ellipsoids  to  Aid  in  Target  Location. 
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Tigure  12.  Actual,  Estimated,  and  Predicted  Tracks:  Target-Range  Observables 
Only.  Actual  Track  -  o,  Estimated  Track  -  +  ,  Predicted  Track  -  * 


Figure  13.  Total  Target  Track  View:  Target-Range  Observables  Only.  Actual 
Track  -  o,  Observed  Track  -  • ,  Sensor  -  x.  Predicted  Track  -  *. 


40 


DISTANCE  X 


o  o  ©  o  o  o  o 

ts  1— I  r-t 

I 


A  HDNVlSia 

Figure  14.  Actual  and  Observed  Tracks:  Target-Bearing  and  Range  Observables. 
Actual  Track  -  o,  Observed  Track  - . ,  Sensor  -  x. 
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Figure  15.  Actual  and  Estimated  Tracks:  Target-Bearing  and  Range  Observables. 

Actual  Track  -  o,  Estimated  Track  -  +  .  The  Plot  Also  Contains  Error 
Ellipsoids  to  Aid  in  Target  Location. 
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rigure  16.  Actual,  Estimated,  and  Predicted  Tracks:  TargetrBearing  and  Range 
Observables.  Actual  Track  -  o,  Estimated  Track  -  +,  Predicted  Track 
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Figure  17.  Total  Target  Track  View:  Target-Bearing  and  Range  Observables. 

Actual  Track  -  o,  Observed  Track  -  •  ,  Sensor  -  x.  Predicted  Track  - 

* 
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4.  Doppler- Frequency  Observable 

The  results  of  a  problem  where  a  Doppler-shifted  frequency  and  the  associated 
bearing  to  that  frequency  are  employed  are  shown  in  Figure  18  on  page  46  through 
Figure  21  on  page  49.  These  are  based  on  a  system  employing  or.c  sensor,  which  is  sta¬ 
tionary  and  located  at  the  origin.  Figure  18  on  page  46  shows  the  actual  track  positions 
compared  to  the  target  observations.  If  the  observations  compare  to  the  actual  posi¬ 
tions,  white  noise  is  added  and  the  observables  sent  to  the  Kalman  algorithm  for  proc¬ 
essing.  Figure  19  on  page  47  shows  the  actual  and  estimated  tracks  with  the  error 
ellipsoids  to  aid  in  defining  target  position.  Figure  20  on  page  48  shows  the  actual,  es¬ 
timated  ,  and  predicted  target  tracks.  Figure  21  on  page  49  is  again  a  combination  of  the 
previous  three  graphs  and  would  be  the  one  seen  in  the  field.  The  target  movement  in 
this  case  was  from  upper  left  to  lower  right. 


45 


oir>o>nO>oO»oo>oo 

i  r- 1 


_  A  HDNVISia 

Figure  18.  Actual  and  Observed  Tracks:  Doppler  Frequency  Observable.  Actual 


Track  -  o,  Observed  Track  •  • ,  Sensor  -  x. 
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ure  19.  Actual  and  Estimated  Tracks:  Doppler  Frequency  Observable.  Ac¬ 
tual  Track  -  o,  Estimated  Track  -  4-  ,  The  Plot  Also  Contains  Error 
Ellipsoids  to  Aid  in  Target  Location. 


Figure  20.  Actual,  Estimated,  and  Predicted  Tracks:  Doppler  Frequency  Ob¬ 
servable.  Actual  Track  -  o,  Estimated  Track  -  +,  Predicted  Track  -  *. 
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Figure  21.  Total  Target  Track  View:  Doppler  Frequency  Observable.  Actual 


Track  -  o,  Observed  Track  -  • ,  Sensor  -  x.  Predicted  Track  -  *. 
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VII.  CONCLUSIONS  AND  RECOMMENDATIONS 

A.  CONCLUSIONS 

The  target  tracking  algorithm  developed  here  is  both  personal  computer-based  and 
user-friendly.  This  would  aid  in  field  employment  where  the  system  would  be  of  most 
use  to  the  maneuver  commander.  With  improvements  in  the  tracking  algorithm  pre¬ 
sented,  or  on  one  similarly  based,  real-time  tracking  could  be  accomplished.  These  im¬ 
provements  will  be  subsequently  discussed.  With  real-time  tracking,  indirect  supporting 
arms  or  direct  air  could  engage  the  target  with  a  reasonable  chance  of  success.  If  the 
orientation  of  the  target  were  known,  optimal  damage  could  then  be  inflicted  upon  it. 
Hence,  the  development  of  a  working  fused-sensor  system  tracking  algorithm  would 
place  forward  reconnaissance  personnel  at  intelligent  risk  when  seeking  combat  maneu¬ 
ver  intelligence  far  fonvard  of  the  FEBA. 

B.  FUTURE  STUDY  POSSIBILITIES 

Future  study  regarding  improvements  in  the  algorithm  developed  is  quite  extensive. 
Topics  which  would  facilitate  the  program's  usefulness  include  incorporation  of  the  ac¬ 
celeration  terms  in  the  X  and  Y  directions  and  the  application  of  the  Z  coordinate  and 
its  velocity  and  acceleration  terms.  PLRS  uses  time  division  multiple  access  (TDMA)  for 
unit  communications  transmissions.  A  delay  study  should  be  done  so  as  to  account  for 
a  worst-case  analysis  of  the  eF  ■ f  that  transmission  delays  would  have  on  the  employ¬ 
ment  of  the  Kalman  tracker  or  us  target  update  capability. 

C.  PROGRAM  IMPROVEMENTS 

Improved  program  structure  can  be  achieved  by  employment  of  the  principles  used 
by  Baheti  [Ref.  8].  The  Baheti  algorithm  requires  one  quarter  of  the  memory  capacity 
and  fewer  computations  than  the  algorithm  as  developed  here,  but  does  not  adhere  to 
conventional  control  methods.  The  algorithm  herein  presented  was  written  in  PC  Matlab 
which  is  a  matrix  manipulation  program.  It  will  require  updating  as  improvements  in  the 
Matlab  algorithm  are  developed. 

Since  optima^  tracking  of  a  ground  target  would  be  futile  in  the  fact  that  the  ground 
target  motion  would  be  unpredictable,  another  possible  program  improvement  could  be 
researched.  The  Defense  Mapping  Agency  has  worldwide  computer  mapping  data  which 
can  be  displayed  on  a  computer  terminal.  It  may  be  possible  to  incorporate  this  data  in 
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a  logic  algorithm  structure  so  as  to  determine  the  target's  tendencies  to  augment  the 
optimal  tracking  scenario.  Once  the  logical  choices  are  made,  the  potential  maneuver 
commander  could  employ  logic  as  well  as  optimal  control  to  thwart  his  adversary  . 
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APPENDIX  TARGET  TRACKING  SIMULATION  PROGRAMS 


A.  PROGRAM  DISCUSSION 

The  programs  contained  herein  were  written  to  simulate  the  varipus  aspects  of  the 
target  tracking  problem  heretofore  discussed.  They  are  written  in  PC-.Matlab  and  hence, 
are  denoted  by  a  .m  suffix  to  identify  then,  as  control  or  execution  files.  A  caveat  should 
be  issued  at  this  point.  This  set  of  programs  was  written  in  PC-Matlab  version  3.5  which 
contains  some  newer  function  files  not  previously  available.  One  should  check  the  ver¬ 
sion  of  PC-Matlab  employed  before  attempting  tracking  simulations.  If  the  function  files 
necessary  to  run  the^e  programs  are  not  present,  an  error  will  result.  The  programs  are 
commented  throughout  in  the  hope  that  they  will  aid  the  user  in  understanding  their 
operation. 

The  main  dm  or  file  for  the  cot  ic  titled  marine. m.  The  subroutines  arc  titled 
brgbrg.m.  rngrng.m.  brgrng.m.  and  doppler.m.  The  file  errellip.m  is  called  by  each  of  the 
subroutine^  as  a  function  fie  The  function  file  reshape. m,  is  also  necessary  for  the  op¬ 
eration  of  marine. m.  It  is  contained  for  reference.  T  hese  programs  should  not  be  con¬ 
sidered  as  validated  or  free  of  logical  or  arithmetic  error. 

B.  PROGRAM  LISTING 


%  JOHN  A.  RUCKS  II 
%  PILE  NAME:  MARINE. M 

X  CONTROL  FILE  FOR  MULTIPLE  GEOMETRY  EXTENDED  KALMAN  FILTER  TARGET 
X  TRACKING  SIMULATION  PROGRAM. 

%  FILES  NECESSARY  FOR  THE  OPERATION  OF  THIS  PROGRAM  ARE: 

X  BRGERG. M 

°o  RN3RNG.  M 

%  EKGRNG.  M 

%  DOPPLER.M 

X  ERRELLIP.M  (COURTESY  OF  STEPHEN  L.  SPEHN) 

X  RESHAPE. M  (FUNCTION  FILE) 

X  THIS  PROGRAM  DRIVES  OTHER  MATLAB  M  (EXECUTION)  FILES  WHICH  SIMULATE 
X  TARGET  TRACKING  BASED  ON  VARIOUS  OBSERVABLES,  THESE  BEING  BEARINGS, 
%  RANGES,  BEARINGS  AND  RANGES,  OR  A  DOPPLER  SHIFTED  FREQUENCY.  THE 
%  PROGRAMS  ARE  BASED  ON  FUSED  MULTIPLE  OBSERVERS  THAT  ARE  EITHER 
X  STATIONARY  OR  MOVING. 

%  PROGRAM  SETUP 


clear, ho Id  off , subplot ( lll),clg,clc 

titleline  =  '  TARGET  TRACKING  SIMULATOR'; 

titlelinel  =  '  (OPTIMAL) 


titleline2  =  ' 
titleline3  =  ' 


NAVAL  POSTGRADUATE  SCHOOL  MSEE  THESIS 
CAPTAIN  JOHN  A.  HUCKS  II  USMC  DEC.  1989 


%  TITLE  DISPLAY 


disp( '  '); 
disp( '  ’); 
disp( '  '  ); 
disp( '  '); 
disp( '  '  ); 
disp('  '); 
disp( '  '); 
disp( '  '); 
dispC  ’); 
disp( '  '); 
disp( '  '); 
disp( title  line); 
disp(titlelinel); 
disp( titleline2); 
disp( t it le 1 ine3) ; 
pause 


OBSERVER  INPUT  ROUTINE 


clc 

cispf 

d  1  s  p  f 

disp( 

disp( 

dispf 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

xfols 

eval(  | 

xfol  = 


’) 

') 

’) 

') 

ENTER  THE  FIRST 


OBSERVERS  INITIAL  PARAMETERS:  '); 


xfol  = 


X 

vx 

y 

vy 


'YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT 
=  input(’xfol  - 
'  xfol  =  '  , xfols 
=  reshape(xfol ,4, 1); 


observer  x  position  (km)  ') 
velocity  x  direction  (km/hr)') 
observer  v  direction  (km)') 
velocity  y  direction  (km/hr)') 

vx  y  vy] ’  ' ) ; 


clc 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 

disp( 


' 

’ ); 

ENTER  THE  SECOND  OBSERVERS  INITIAL  PARAMETERS:  ’); 


xfo2  =  [ 


X 

vx 

y 

vy 


]; 


%  observer  x  position  (km)’) 

%  velocity  x  direction  (km/hr)’) 
%  observer  y  direction  (km)  ’) 

%  velocity  y  direction  (km/hr)’) 


’); 

YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT:  [  x  vx  y  vy]”); 


xfo2s  =  input('xfo2  = 
eval(['xfo2  =  ’ ,xfo2s , ’ ; ' ] ); 
xfo2  =  reshape(xfo2,4,l); 


clc 

disp( 1  '); 

dispC 'ENTER  OBSERVER  MOVEMENT  SPEED: '); 
dispC  '); 

disp( '  spd 

dispC  '); 
dispC ’  '); 

spds  =  inputC  spd  =  ','s'); 
eval([  'spd  =  '  ,spds, ';']); 


%  observer  speed  in  km/hr  ' ) 


clc 

dispC  '); 

disp( ' ENTER  DESIRED  NUMBER  OF  OBSERVATIONS:'); 
disp('  '); 

disp( '  kmax  %  observation  number  ) 

dispC  '); 
dispC  '); 

dispC PLEASE  MAKE  THIS  GREATER  THAN  5  SO  THAT  THE  KALMAN  GAINS') 

dispC  CAN  STABILIZE.  '  ) 

kmaxs  =  ir.put('kmax  =  ','s'); 

eval( [ ' kmax  =  ' , kmaxs ,';']); 


clc 

disp( '  ’); 

disp( 'ENTER  DESIRED  NUMBER  OF  PREDICTIONS:'); 
dispC  '); 

disp( '  kpred 

disp('  ’); 
cispC’  ’); 

kpreds  =  inputC kpred  =  ','s'); 
eval( [' kpred  =  '.kpreds,';']); 


prediction  number  ') 


clc 

dispC’  '); 

dispC 'ENTER  OBSERVATION  INTERVAL:'); 
disp(’  '); 

dispC'  dt  %  observation  interval') 

dispC  '); 

dispC 'THE  TIME  INTERVAL  IS  IN  THE  FORM:') 
dispC'  ') 

dispC'  dt  =  time/60') 

dispC  '); 

dispC ' FOR  A  10  MINUTE  INTERVAL  ENTER:  dt  =  10/60’) 
dts  =  inputC 'dt  =  ','s'); 
eval( [ 'dt  =  '.dts,'; '1 ); 


%  TARGET  SIMULATION  PARAMETERS  INPUT  ROUTINE 


clc 
dispC ' 
dispC ' 
dispC ' 


FOR  THE  PURPOSES  OF  SIMULATION,'); 


disp('  ENTER  THE  TARGETS  MOVEMENT  PARAMETERS:  '); 

disp(’  '); 
disp( '  '); 

disp( '  [  x  %  target  initial  x  position  (km)') 

disp( '  vx  %  target  initial  x  velocity  (km/hr)') 

disp( '  ax  %  target  acceleration  in  x  (km/hr*2)') 

disp( '  y  %  target  initial  y  position  (km)’) 

disp( '  vy  %  target  initial  y  velocity  (km/hr)') 

disp( 1  ay  %  target  acceleration  in  y  (km/hrA2)') 

disp( '  fo  ] ;  %  transmitter  rest  frequency  (Hz)') 

disp( '  '); 
disp( '  '); 

disp( ' YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT:  [ x  vx  ax  y  vy  ay  fo]  "  ); 
disp( '  '); 
dispC  '); 

disp( '  *****************************************************  ' ) 

disp( '  *  CAUTION:  VELOCITY  OF  THE  TARGET  MUST  NOT  BE  ZERO.  *  ' ) 

disp( '  *****************************************************  ' ) 

tgts  =  input ( ' tgt  =  ','s'); 
eval( c ' tgt  =  ' 3 tgt s , ' ;  '  | ); 
tgt  =  reshape( tgt ,7,1); 
if  (tgt(  2 , 1 )  =  0  £.  tgt(  5 , 1)  =  0) 
clc 


x  %  target  initial  x  position  (km)') 

vx  %  target  initial  x  velocity  (km/hr)') 

ax  %  target  acceleration  in  x  (km/hr/v2)') 
y  %  target  initial  y  position  (km)’) 

vy  %  target  initial  y  velocity  (km/hr)') 

ay  %  target  acceleration  in  y  (km/hrA2)') 
fo  ] ;  %  transmitter  rest  frequency  (Hz)') 


disp( '  '); 
disp( '  '); 

dispC  FOR  THE  PURPOSES  OF  SIMULATION,’); 

dispC  ENTER  THE  TARGETS  MOVEMENT  PARAMETERS:  '); 

disp( '  '); 
disp((  ); 

disp(  '  [  x  °0  target  initial  x  position  (kn 

disp( '  vx  %  target  initial  x  velocity  (kn 

dispf '  ax  %  target  acceleration  in  x  (km/ 

disp( '  y  %  target  initial  y  position  (kn 

disp( '  vy  %  target  initial  y  velocity  (kn 

disp( '  ay  %  target  acceleration  in  y  (km/ 

disp( '  fo  ]  ;  %  transmitter  rest  frequency  (I 

disp( '  '); 
disp( '  '); 

disp( ' YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT: 

[  x  vx  ax  y  vy  ay  foj  ' ' ); 
disp( '  '); 
disp( '  '); 

^  ^  5  p  ^  1  it  Vr Vr  Vr  Vr  iz  tV  it  it  it  it  it  it  it  it  it  7V  it  it  it  it  it  it  it  it  it  it  it  it  it  it  it  Vr  “V  tV  it  it  it  it  it  it  it  it  it  it  it  it  it  it  it  it 

disp( '  *  CAUTION:  VELOCITY  OF  THE  TARGET  MUST  NOT  BE  ZERO.  * 
d i s  p ( '  ***************************************************** 
tgts  =  input ( ' tgt  =  ','s'); 
eval( [ 'tgt  =  ' , tgts , ' ; '] ); 
tgt  =  reshape( tgt ,7,1); 


°o  target  initial  x  position  (km)') 

%  target  initial  x  velocity  (km/hr)') 

?0  target  acceleration  in  x  (km/hr  -, 2)  ’ ) 
%  target  initial  y  position  (km)') 

%  target  initial  y  velocity  (km/hr) ') 

%  target  acceleration  in  y  (km/hr- 2)') 
%  transmitter  rest  frequency  (Hz)') 


global  xfol  xfo2  spd  kmax  kpred  dt  tgt; 


%  OBSERVABLE  MENU  CHOICE  ROUTINE 


choicel  =  -10; 
while  choicel  =  0 
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disp(  '  '); 

disp('  '); 

dispC '  '); 

disp(titleline); 

disp( titlelinel); 

dispC '  ’); 

dispC  ’); 

dispC  ’); 

disp( ' 

dispC  '); 

dispC 1 

disp( ' 

disp( ' 

dispC ' 

dispC 1 

disp( ' 

disp( ' 

dispC ' 

dispC ' 

disp( ' 

disp( ’ 

dispC ' 

dispC '  ' ); 

disp( ' 

dispC'  '); 


ENTER  THE  METHOD  OF  OBSERVATION  USED  FOR  THIS  RUN: '); 

1.  Bearing/Bearing' ); 

2.  Bearing/Range'); 

3.  Range/Ranger); 

4.  Shifted  Doppler  Frequency’); 

5.  Display  the  last  graph'); 

6.  Change  the  number  of  observations'); 

7.  Change  the  number  of  predictions'); 

8.  Change  the  observation  interval'); 

9.  Change  position  of  first  observer'); 

10.  Change  position  of  second  observer'); 

11.  Change  observer  movement  speed'); 

12.  Change  target  simulation  parameters'); 

0.  Quit'); 

1  ENTER  YOUR  CHOICE:  ’); 


choicel  =  inputC  ENTER  YOUR  CHOICE:  ’); 

if  choicel  ==  1 
brgbrg; 

elseif  choicel  =  2 
brgrng; 

elseif  choicel  =  3 
rngrng; 

elseif  choicel  ==  4 
doppler; 

elseif  choicel  ==  5 
shg 
pause 

elseif  choicel  =  6 
clc 

dispC'  '); 

dispC 'ENTER  DESIRED  NUMBER  OF  OBSERVATIONS:  '); 
dispC |  '); 

dispC 1  kmax  %  observation  number  ’ ) 

dispC’  ’); 
dispC’  ’); 

dispC ’PLEASE  MAKE  THIS  GREATER  THAN  5  SO  THAT  THE  KALMAN  GAINS') 
dispC 'CAN  STABILIZE. ' ) 
kmaxs  =  input('kmax  =  ','s'); 
eval( [ ' kmax  =  ' , kmaxs ,';']); 
elseif  choicel  =  7 
clc 

dispC'  '); 

dispC 'ENTER  DESIRED  NUMBER  OF  PREDICTIONS: '); 
dispC |  '); 

dispC'  kpred  %  prediction  number  ') 
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dispC '  '); 
dispC  '); 

kpreds  =  input('kpred  =  ','s'); 
eval(['kpred  =  ', kpreds, ';'] ); 
elseif  choicel  =  8 
clc 

disp('  '); 

disp( 'ENTER  OBSERVATION  INTERVAL:'); 
dispC'  ') 

disp('  dt  %  observation  interval  ) 

dispC'  ’); 

dispC 'THE  TIME  INTERVAL  IS  IN  THE  FORM:  ’) 
dispC'  ') 

dispC'  dt  =  time/60') 

dispC'  ’); 

dispC' FOR  A  10  MINUTE  INTERVAL  ENTER:  dt  =  10/60') 
dispC  '); 

dts  =  inputC'dt  =  ','s'); 

evalCI'dt  *  ’,dts, 

elseif  choicel  =  9 
clc 

dispC'  '); 
dispC  '); 
dispC  ’); 

dispC  '); 

dispC 'ENTER  THE  FIRST  OBSERVERS  INITIAL  PARAMETERS:’); 
dispC’  '); 
dispC ’  '); 

dispC'  xfol  =  [  x  %  observer  x  position  (km)  ') 

dispC'  vx  %  velocity  x  direction  (km/hr)  ') 

disp('  y  %  observer  y  direction  (km)') 

dispC'  vy  ] ;  %  velocity  y  direction  (km/hr)') 

dispC  '); 

dispC 'YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT:  [  x  vx  y  vy]  "); 
xfols  =  input('xfol  =  ','s'); 
eval(['xfol  =  ', xfols ,';'] ); 
xfol  =  reshapeCxfol ,4, 1); 
elseif  choicel  =  10 
clc 

disp('  '); 
dispC'  '); 
dispC'  '); 
disp('  '); 

dispC 'ENTER  THE  SECOND  OBSERVERS  INITIAL  PARAMETERS:'); 

dispC  '); 

dispC  '); 

dispC’  xfo2  =  [  x  %  observer  x  position  (km)') 

dispC'  vx  %  velocity  x  direction  (km/hr)’) 

dispC'  y  %  observer  y  direction  (km)') 

dispC'  vy  ] ;  %  velocity  y  direction  (km/hr)') 

dispC  ’); 

dispC 'YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT:  [  x  vx  y  vy]  ”); 
xfo2s  =  input('xfo2  =  ','s'); 
eval([ 'xfo2  =  ' ,xfo2s , ' ; ' ] ); 
xfo2  =  reshapeC xfo2 , 4 , 1); 
elseif  choicel  =  11 
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clc 

disp( '  '); 

disp(' ENTER  OBSERVER  MOVEMENT  SPEED: '); 

dispC  '); 

disp( '  spd  %  observer  speed  in  km/hr  ' ) 

dispC  '); 
disp(’  '); 

spds  =  input( 1 spd  =  ','s'); 
eval([  'spd  =  ’ ,spds, '; '] ); 
elseif  choicel  =  12 
clc 

disp( 1  '); 

dispC  ’); 

dispC  FOR  THE  PURPOSES  OF  SIMULATION,'); 

dispC  ENTER  THE  TARGETS  MOVEMENT  PARAMETERS:'); 

dispC  '); 
dispC  '); 

disp( '  [  x  X  target  initial  x  position  (km)') 

disp( '  vx  %  target  initial  x  velocity  (km/hr)') 

dispC  ax  %  target  acceleration  in  x  (km/hr^Z)') 

disp( '  y  %  target  initial  y  position  (km)') 

disp( '  vy  %  target  initial  y  velocity  (km/hr)') 

disp( '  ay  %  target  acceleration  in  y  (km/hrA2)’) 

disp('  fo  ] ;  °0  transmitter  rest  frequency  (Hz)') 

dispf'  ’  '  ) ; 
dispC '  ' ) : 

dispC 'YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT: 

[x  vx  ax  y  vy  ay  fo] '') 


dispC'  '); 
dispC'  '); 

dispC  '  *********"'•'■''■  •’■■'?*'‘'***'!'f*******'!''***************y r*Vr ******* 

dispC |  *  CAUTION:  VELOCITY  OF  THE  TARGET  MUST  NOT  BE  ZERO.  * 

tgts  =  input ( 'tgt  =  ','s'); 
eval([ 'tgt  =  ' , tgts , ' ; '] ); 
tgt  =  reshapeC  tgt ,7,1); 

if  ( tgt( 2 , 1)  ==  0  &  tgt(5,l)  =  0) 
clc 


’) 

') 

’) 


dispC  '); 
dispC '  ’); 

dispC’  FOR  THE  PURPOSES  OF  SIMULATION,’); 

dispC'  ENTER  THE  TARGETS  MOVEMENT  PARAMETERS: '); 

dispC  '); 
dispC'  '); 

dispC'  [  x  %  target  initial  x  position  (km)') 

dispC'  vx  %  target  initial  x  velocity  (km/hr)') 

dispC'  ax  %  target  acceleration  in  x  (km/hrA2)') 

dispC  y  %  target  initial  y  position  (km)') 

disp('  vy  %  target  initial  y  velocity  (km/hr)') 

dispC'  ay  %  target  acceleration  in  y  (km/hrA2)') 

dispC  fo  ] ;  %  transmitter  rest  frequency  (Hz)') 

disp(’  '); 
disp('  '); 

dispC 'YOU  MAY  ENTER  THIS  IN  THE  FOLLOWING  FORMAT: 

[x  vx  ax  y  vy  ay  fo]  '  '  ); 
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dispC  '); 

disp(  |  ^  ,  ,  .  ,  ,  ,  ,  AA  A  .  A A  .  A  .  A ;  ,  AAA-A-.AA-.AA  .  .  AA  .  AA  .  AAAAA^AAA,.  ■ 

dispC  *  CAUTION:  VELOCITY  OF  THE  TARGET  MUST  NOT  BE  ZERO.  *’) 

tgts  =  input('tgt  =  '  , ' s ' ); 
eval( [ 'tgt  =  ' ,tgts, ' ; '] ); 
tgt  =  reshape(tgt ,7,1); 

end 

end; 

end; 


%  JOHN  A.  HUCKS  II 
%  FILE  NAME  :  BRGBRG. M 

%  EXTENDED  KALMAN  FILTER  PROGRAM  USING  BEARING/BEARING  MEASUREMENTS 

%  THIS  PROGRAM  SIMULATES  THE  TRACK  GENERATED  BY  AN  EXTENDED  KALMAN 
%  FILTER,  WITH  INPUTS  FROM  A  FUSED  SENSOR  ARRAY  AS  IT  TRACKS  A  TARGET 
?o  ACROSS  AN  X-Y  GRID  SPACE  (PLRS  FLAT  EARTH  PROJECTION). 


clg 

clear  eex  eey  xto 
! erase  brgbrg. met 
diary  bearing 

%  STATE  EQUATIONS 

%  OBSERVATION  TIME  INTERVAL  (input  from  marine. m) 

%  STATE  TRANSITION  MATRIX 
phi  =  [  1  dt  0  0 

0  10  0 
0  0  1  dt 

0  0  0  1  ]  ; 


0/ 

'0 


del 


SYSTEM  NOISE  COEFFICIENT  MATRIX 
=  [  dt-dt/2  0 

dt  0 

0  dt*dt/2 

0  dt  ]; 


%  STATE  VARIABLES,  FILTER  ESTIMATES,  TIME,  AND  OBSERVED  DATA 
%  NUMBER  OF  OBSERVATIONS  (input  from  marine. m) 


%  ESTIMATED  TARGET  STATE 

xte  =  zeros(4,kmax);  %  initial  est.  target  state 

%  OBSERVER  (SENSOR)  INITIAL  POSITIONS  AND  VELOCITIES 
%  (input  from  marine. m) 

%  OBSERVER  SPEED  (input  from  marine. m) 
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%  INITIAL  TIME  SETTING 
Time  =  zeros( 1 ,kmax) ; 


%  ACTUAL  TRACK  POSITIONS  (tgt  vector  input  from  marine. m) 
x  =  zeros( 1 ,kmax); 
y  =  zeros( 1 ,kmax); 

T  =  0; 
for  k  =  1 

x(k)  =  tgt( 1,1); 
y(k)  =  tgt(4,l); 

end 

for  k  =  2:  kraax 

T  =  T+dt; 

x(k)  =  ( 0.  5*tgt( 3 ,1)*(Ta2))+( tgt( 2,1).  *T)+( tgt( 1,1)); 
y(k)  =  (0.5*tgt(6,l)*(TA2))+(tgt(5,l).*T)+(tgt(4,l)); 

end 

zpos  =  [  x 

y  I; 

%  INPUT  OBSERVATION  VALUES  (BEARING/BEARING) 

°0  INITIAL  OBSERVED  TARGET  STATE 

xto(:  ,1)  =  [  tgt(l,l)  %  (km  in  x  direction) 

tgt(2,l)  %  (km/hr  in  x  direction) 

tgt(Ajl)  %  (km  in  y  direction) 

tgt(5,l)  J;  %  (km/hr  in  y  direction) 

SIMULATION  COUNTER  LOOP 
xfoa  =  zeros(4,kmax); 
xfoa( : , 1)  =  xfol( : , 1); 
xfol  =  xfoa; 
xfob  =  zercs( 4 ,kmax) ; 
xfob(: ,1)  =  xfo2(: ,1); 
xfo2  =  xfcb; 
k  =  C; 

for  kl=l: 10*kmax 
k  =  k  +  1; 
if  (k  ==  kmax) 
break 
end 

?,  UPDATE  FORWARD  OBSERVER'S/  FAC'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 
way  =  pi/2; 
else 

way  =  -( 3*pi/4); 
end 

if  spd  =  0 

xfol(l,k+l)  =  xfol(l,k); 
xfol(3,k+l)  =  xfol(3,k); 
xfo2( 1 ,k+l)  =  xfo2(  1 ,k); 
xfo2(3,k+l)  =  xfo2(3,k); 
else 

xfol(l,k+l)  =  xfol(l,k)  +  spd*sin(way)*dt; 
xfol(2,k+l)  =  spd*sin(way); 
xfol(3,k+l)  =  xfol(3,k)  +  spd*cos( way)*dt; 
xfol(4,k+l)  =  spd'-':cos(way) ; 


%  actual  loop  counter 
%  check  for  termination 
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xfo2(l,k+l)  =  xfo2(l,k)  +  spd*sin(way)*dt; 
xfo2(2,k+l)  =  spd*sin(way); 
xfo2(3,k+l)  =  xfo2(3,k)  +  spdlVcos(way)*dt; 
xfo2(4,k+l)  =  spd*cos(way); 
end 
end 

deltlx  =  zpos(l,:)  -  xfol(l,:); 
deltly  =  zpos(2,:)  -  xfol(3,:); 
delt2x  =  zpos(l,:)  -  xfo2(l,:); 
delt2y  =  zpos(2,:)  -  xfo2(3,:); 

brgl  =  atan2(deltlx, deltly);  %  bearing  thtal 

brg2  =  atan2(delt2x,delt2y);  %  bearing  thta2 

zobs  -  [  brgl; brg2] ; 

xtol  =  zeros(4 ,kmax); 

xtol(l,:)  =  zpos(l,:); 

xtol(3,:  )  =  zpos(2,:  ); 

xtol( : , 1)  =  xto( : , 1); 

xto  =  xtol; 

%  STATE  EXCITATION  AND  MEASUREMENT  ERROR  COVARIANCE  MATRICES 
varv  =  0. 0001;  %  variance  of  linear  acceleration 

varth  =  0.01096;  %  variance  of  angular  acceleration 

R  =  [  0. 000005  0 

0  0. 000005  ] ;  %  measurement  noise 

%  INPUT  OBSERVATION  VALUES  WITH  NOISE  ADDED 
rand( ' normal ' ) 

zobs  =  zobs  +  sqrt(R(  1 , 1)  )'-vrand(zobs); 

%  INITIAL  KALMAN  MATRICES  AND  OBSERVATION 
I  =  eye( 4) ; 

%  INITIAL  KALMAN  P  MATRIX 

P0  =  [  800  000  %  reset  value  of  Error  Covariance 

0  800  0  0 

0  0  800  0 

0  0  0  800  ] ; 

%  REINITIALIZING  P  MATRIX 
P  =  P0; 

%  KALMAN  ESTIMATE 

xte(:  ,1)  =  [  xto(l,l)  %  (km  -  x  direction) 

tgt(  2,1)  %  (  -  .n/hr) 

xto(3,l)  %  (km  -  y  direction) 

tgt( 5 , 1 )  ] ;  %  (km/hr) 

tpred  =  xte( : , 1 ) ; 

%  SIMULATION  OF  KALMAN  FILTER  OPERATION 
%  NUMBER  OF  BAD  OBSERVATIONS 

nbad  =0;  %  initial  setting  bad  obs.  cntr 

%  SIMULATION  COUNTER  LOOP 
k  =  0; 

for  k  1  =  1 :  10*kmax 


%  actual  loop  counter 
%  check  for  termination 


k  =  k  +  1; 
if  (k  ==  kmax) 
break 

end 

%  UPDATE  FORWARD  OBSERVER'S/  FAC’S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 
way  =  pi/2; 
else 

way  =  -(3*pi/4); 

end 

if  spd  ==  0 

xfol(l,k+l)  =  xfol(l,k); 
xfol(3,k+l)  =  xfol( 3 ,k); 
xfo2(l,k+l)  =  xfo2( 1 ,k); 
xfo2(3,k+l)  =  xfo2(3,k); 

6  1 S  6 

xfol(l,k+l)  =  xfol(l,k)  +  spd*sin(way)*dt; 

xfol(2,k+l)  =  spd*sin(way); 

xfol(  3 ,k+l )  =  xfol(3,k)  +  spd*cos(way)*dt; 

xfol(4,k+l)  =  spd'vcos(  way) ; 

xfo2(l,k+l)  =  xfo2(l,k)  +  spd*sin(way)*dt; 

xfo2(2,k+l)  =  spd*s in( way ) ; 

xfo2(3,k+l)  =  xfo2(3,k)  +  spd*cos(  way  ),vdt; 

xfo2(4,k+l)  =  spd*cos( way) ; 

end 

°c  PROJECT  AHEAD  KALMAN’  STATE  ESTIMATE 
xte(:  ,k+l)  =  phi*xte(:  ,k); 

%  UPDATE  Q 

vt  =  sqrt( (xte( 2 ,k+l) ) '2  +  ( xte( 4 ,k+l) ) a2) ; 
qll  =  varv  *  (xte(2,k+l)  /  vt)/2  +  varth  *  (xte(4,k+l))A2; 
q22  =  varv  *  (xte(4,k+l)  /  vt)/.2  +  varth  ,v  ( xte(  2 ,k+l ) )  a2; 
ql2  =  xte(2,k+l)  *  xte(4,k+l)  *  ((varv/vt)A2  -  varth  ); 
q21  =  q 1 2 ; 

Q1  =  [  qll  ql2 

q21  q22  ]■, 

Q  =  del*Ql*(del  ); 

%  PROJECT  ERROR  COVARIANCE 
P  =  phi*P*(phi' )  +  Q; 

%  UPDATE  H 

deltlx  =  xte(l,k+l)  -  xfol(l,k+l); 
deltly  =  xte(3,k+l)  -  xfol(3,k+l); 
delt2x  =  xte(l,k+l)  -  xfo2(l,k+l); 
delt2y  =  xte(3,k+l)  -  xfo2(3,k+l); 
r 12  =  (deltlx)  2  +  (deltly)  2; 
r22  =  (delt2x)  2  +  (delt2y)  2; 
rl  =  sqrt(rl2); 
r2  =  sqrt(r22); 

H  =  [  deltly/rl2  0  -deltlx/rl2  0 

delt2y/r22  0  -delt2x/r22  0); 

%  GET  NEW  MEASURED  ESTIMATE 
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zhatl  =  atan2(deltlx,deltly);  %  bearing  estimate  thtal 

zhat2  =  atan2(delt2x,delt2y);  %  bearing  estimate  thta2 

zhat  =  [  zhatl 

zhat2  ] ; 

%  COMPUTATION  OF  KALMAN  GAIN 

vresid  =  (H*P,V(H')  +  R);  %  variance  of  the  residual 

G  =  P*(H' )  /  vresid;  %  Kalman  gain 

%  COMPUTATION  OF  UPDATED  ERROR  COVARIANCE 
P  =  (I  -  G*H)*P; 

%  COMPUTATION  OF  RESIDUAL 
resid  =  zobs(: ,k+l)  -  zhat; 

%  IF  RESIDUAL  IS  TOO  BIG,  THEN  RESET  P  AND  DO  CALCULATIONS  AGAIN, 
if  (abs( resid( 1) )  >  1.  0*sqrt( abs(vresid( 1 , 1) ) )  ... 

]  abs( resid( 2) )  >  1. 0*sqrt(abs(vresid(2,2)))  ... 

&  nbad  =0) 
nbad  =  1; 
k  =  k-1; 

P  =  PO;  %  reset  P 

else 

COMPUTATION  OF  UPDATED  ESTIMATE 
xte(: ,k+l)  =  xte(: ,k+l)  +  G*resid; 

?0  ERROR  ELLIPSOID  PLOTTING  ROUTINE 
M  =  [  xte(l,k+l)  xte(3,k+l)  ]'; 

K  =  [  PCl.lj  P( 1 , 3) 

PC3.1)  PC  3 , 3)  ]; 

[xp,yp]  =  errel lip( M ,K , . 60 ,50 , 0) ; 
eex( :  ,k)  =  xp; 
eey(: ,k)  =  yp; 

%  PREDICTED  (FUTURE  TRACK)  USING  KALMAN  ESTIMATE 
%  (number  of  predictions  desired  input  from  marine. m) 
tpred(: ,1)  =  xte(: ,k+l); 
acc  =  zeros( 2 , 1 ) ; 

for  1  =  l:kpred  %  kpred  is  #  of  predictions 

if  k  >=  3 

accx  =  ( xte( 2 , k+1) -xte( 2 ,k) )/dt; 
accy  =  ( xte( 4 ,k+l ) -xte( 4 ,k) )/dt; 
acc  =  [ accx; accy]  ; 
if  accx  >=  5 
break 

acc  =  zeros(2 , 1); 
elseif  accy  >=  5 
break 

acc  =  zeros( 2 ,1); 

end 

end 

tpred(: ,1+1)  =  phi*tpred( : , 1 )+del*acc; 

end 

nbad  =  0; 

end 

%  PLOT  OBSERVED  AND  ESTIMATED  TRACKS  (TARGET  AND  OBSERVER) 
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plot(xfol(l,i:k+l),xfol(3,l:k+l),'xw' ,xto( 1 , 1: k+1) ,xto( 3, 1: k+1) 

I  i  f 

>  «  D  j  •  •  • 

xte( 1 , 1:  k+1) ,xte( 3 , 1:  k+1) , ' +w' ,xfo2( 1 , 1:  k+1) ,xfo2( 3, 1:  k+1) . ' xw’ , . .  . 
zpos( 1 ,1: k+1) , zpos( 2 , 1: k+1) , ' ow '  ,tpred( 1 ,: ) ,tpred(3,:  ) , '*g  , .  .  . 
eex.eey , ' -r' ) ,grid 

title( ’ACTUAL/OBSERVED/ESTIMATED/PREDICTED  TGT  TRKS  USING  BRG/BRG  ') 
xlabeK 'DISTANCE  X' ) ,ylabel( ' DISTANCE  Y') 

end 

meta  brgbrg 


%  JOHN  A.  HUCKS  II 
%  FILE  TITLE:  RNGRNG. M 

%  EXTENDED  KALMAN  FILTER  PROGRAM  USING  RANGE/RANGE  MEASUREMENTS 

O/ 

%  THIS  PROGRAM  SIMULATES  THE  TRACK  GENERATED  BY  AN  EXTENDED  KALMAN 
%  FILTER,  WITH  INPUTS  FROM  A  FUSED  SENSOR  ARRAY  AS  IT  TRACKS  A  TARGET 
°„  ACROSS  AN  X-Y  GRID  SPACE  (PLRS  FLAT  EARTH  PROJECTION). 


clg 

clear  eex  eey  xto 
!  erase  rngrng.  met 
diary  rngrng 

%  STATE  EQUATIONS 

%  OBSERVATION  TIME  INTERVAL  (input  from  marine,  m) 

°i  STATE  TRANSITION  MATRIX 
phi  =  [  1  dt  0  0 

0  10  0 
0  0  1  dt 

0  00  1  ]; 

%  SYSTEM  NOISE  COEFFICIENT  MATRIX 


del  =  [  df-dt/2  0 

dt  0 

0  dt*dt/2 

0  dt 


%  STATE  VARIABLES,  FILTER  ESTIMATES,  TIME,  AND  OBSERVED  DATA 


%  NUMBER  OF  OBSERVATIONS  (input  from  marine. m) 

1  ESTIMATED  TARGET  STATE 

xte  =  zeros(4,kmax);  %  initial  est.  target  state 

%  OBSERVER  (SENSOR)  INITIAL  POSITIONS  AND  VELOCITIES 
%  (input  from  marine. m) 

%  OBSERVER  SPEED  (input  from  marine. m) 


M 


%  INITIAL  TIME  SETTING 
Time  =  zeros( 1 ,kmax); 

%  ACTUAL  TRACK  POSITIONS  (tgt  vector  input  from  marine. m) 
x  =  zeros( 1 ,kmax); 
y  =  zeros( 1 ,kmax); 

T  =  0; 
for  k  =  1 

x(k)  =  tgt( 1,1); 
y(k)  =  tgt( 4 , 1 ) ; 

end 

for  k  =  2: kmax 

T  =  T+dt; 

x(k)  =  ( 0.  5*tgt( 3 ,1)*(Ta2))+( tgt( 2 ,1). *T)+( tgt( 1,1)); 
y(k)  =  ( 0.  5*tgt( 6, 1)*(Ta2) )+( tgt(5 , 1). *T)+(tgt(4, 1) ); 

end 

zpos  =  [  x 

y  ]; 

?„  INPUT  OBSERVATION  VALUES  (RANGE/RANGE) 

°0  OBSERVED  TARGET  STATE 
xfoa  =  zeros(4,kmax); 
xfoa( : , 1)  =  xfol( : ,1); 
xfol  =  xfoa; 
xfob  =  zeros(4 ,kmax); 
xfob(: ,1)  =  xfo2(: ,1); 
xfo2  =  xfob; 

xto( :  ,  1)  =  [  tgt(l,l)  %  (km  in  x  direction) 

tgt(2,l)  %  (km/hr  in  x  direction) 

tgt( 4 , 1 )  %  (km  in  y  direction) 

tgt( 5 , 1)  ];  %  (km/hr  in  y  direction) 

%  SIMULATION  COUNTER  LOOP 
k  =  0; 

for  k 1=1 :  10*kmax 

k  =  k  +  1;  %  actual  loop  counter 

if  (k  —  kmax)  %  check  for  termination 

break 

end 

%  UPDATE  FORWARD  OBSERVER’S/  FAC’S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 
way  =  pi/2; 
else 

way  =  -( 3*pi/4); 

end 

if  spd  =  0 

xfol(l,k+l)  =  xfol(l,k); 
xfol(3,k+l)  =  xfol(3,k); 
xfo2(l,k+l)  =  xfo2(l,k); 
xfo2( 3 ,k+l)  =  xfo2( 3 ,k) ; 
else 

xfol(l,k+l)  =  xfol(l,k)  +  spd*sin(way)*10*dt; 
xfol(2,k+l)  =  spd*sin(way); 

xfol(3,k+l)  =  xfol(3,k)  +  spd*cos(way)*10*dt; 
xfol(4,k+l)  =  spd*cos(way); 

xfo2(l,k+l)  =  xfo2(l,k)  +  spd*sin(way)lPTO*dt; 


xfo2(2,k+l)  =  spd*sin(way); 

xfo2(3,k+l)  =  xfo2(3,k)  +  spd*cos(way)*10*dt; 
xfo2(4,k+l)  =  spd*cos(way); 

end 

end 

rl  =  sqrt( ( zpos( 1 , : ) -xfol( 1 , : ) ) . a2  +  (zpos(2,: )-xfol(3,: )). a2); 

r2  =  sqrt( ( zpos( 1 , : ) -xfo2( 1 , : ) ) . a2  +  (zpos(2,: )-xfo2(3,: )). a2); 

zobs=[ rl;  r2]  ; 

xtol  =  zeros(4,kmax); 

xtol(l,:  )  =  zpos(l,:  ); 

xtol(3,:)  =  zpos(2,:  ); 

xtol( :  ,1)  =  xto(:  ,1); 

xto  -  xtol; 

%  STATE  EXCITATION  AND  MEASUREMENT  ERROR  COVARIANCE  MATRICES 

varv  =5.0;  %  variance  of  linear  acceleration 

varth  =  1. 0;  %  variance  of  angular  acceleration 

R  =  [  0.  005  0 

0  0.  005  ]  ;  7o  measurement  noise 

%  .NPUT  OBSERVATION  VALUES  WITH  NOISE  ADDED 
rand( ' normal 1 ) ; 

zobi  =  zobs  +  sqrt(R( 1 , 1) )*rand(zobs); 


%  INITIAL  KALMAN  MATRICES  AND  OBSERVATION 


I  =  eye( 4) ; 

%  INITIAL  KALMAN  P  MATRIX 
P0  •=  [  250  0  0  0 

0  250  0  0 

0  0  250  0 

0  0  0  250  ] ; 

°o  REINITIALIZING  P  MATRIX 
P  -  P0; 

%  KALMAN  ESTIMATE 
xtef : , 1)  =  [  xto( 1,1) 
tgt( 2,1) 
xto( 3,1) 
tgt(5,l)  J; 


n  reset  value  of  Error  Covariance 


%  (km  *  x  direction) 
%  (km/hr) 

%  ( km  -  y  direction) 
%  (km/hr) 


tp-ed  =  xte( :  , 1 ) ; 


SIMULATION  OF  KALMAN  FILTER  OPERATION 
°o  NUMBER  OF  BAD  OBSERVATIONS 

nba  i  =  0;  %  initial  setting  bad  obs.  cntr. 

SIMULATION  COUNTER  LOOP 
k  =  0; 

for  kl=l: 10*kmax 

k  =  k  +  1;  X  actual  loop  counter 

if  (k  =  kmax)  X  check  for  termination 

break 

end 


%  UPDATE  FORWARD  OBSERVER’S/  FAC’S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 


way  =  pi/2; 
else 

way  =  -(3*pi/4); 

end 

if  spd  ==  0 

xfol(l,k+l)  =  xfol(l,k); 
xfol(3,k+l)  =  xfol(3,k); 
xfo2(l,k+l)  =  xfo2(l,k); 
xfo2(3,k+l)  =  xfo2( 3 ,k); 
else 

xfol(l,k+l)  =  xfol(l,k)  +  spd*sin(way)*10*dt; 
xfol(2,k+l)  =  spd*sin(way); 
xfol(3,k+l)  =  xfol(3,k)  +  spd*cos(way)*10*dt; 
xfol(4,k+l)  =  spd*cos(way); 

xfo2(l,k+l)  =  xfo2(l,k)  +  spd*sin(way)*10*dt; 
xfo2(2,k+l)  =  spd*sin(way); 

xfo2(3,k+l)  =  xfo2(3,k)  +  spd*cos( way)*10*dt; 
xfo2(4,k+l)  =  spd*cos(way); 

end 

%  PROJECT  AHEAD  KALMAN’  STATE  ESTIMATE 
xte(: ,k+l)  =  phi*xte(: ,k); 

%  UPDATE  Q 

vt  =  sqrt( (xte( 2 ,k+l) )  2  +  (xte(4,k+l))  2); 
q 1 1  =  varv  *  (xte(2,k+l)  /  vt)  2  +  varth  *  (xte(4,k+l))  2; 
q22  =  varv  *  (xte(4,k+l)  /  vt)  2  +  varth  *  (xte(2,k+l))  2; 
ql2  =  xte(2,k+l)  *  xte(4,k+l)  *  ((varv/vt)  2  -  varth  ); 
q21  =  ql2; 

Q1  =  [  qll  ql2 

a21  q22  ]; 

Q  =  del';:Ql*Cdel^  ); 

%  PROJECT  ERROR  COVARIANCE 
P  =  phi*P*( phi ' )  +  Q; 

%  UPDATE  H 

deltlx  =  xte(l,k+l)  -  xfol(l,k+l); 
deltly  =  xte(3,k+l)  -  xfol(3,k+l); 
delt2x  =  xte(l,k+l)  -  xfo2(l,k+l); 
delt2v  =  xte(3,k+i)  -  xfo2(3,k+l); 
r 12  =” (deltlx)  2  +  (deltly)  2; 
r22  =  (delt2x)  2  +  (delt2y)  2; 
rl  =  sqrt(rl2); 
r2  =  sqrt(r22); 

H  =  [  deltlx/rl  0  deltly/rl  0 

delt2x/r2  0  delt2y/r2  0  ]; 

%  GET  NEW  MEASURED  ESTIMATE 
zhat  =  [  rl 

r2  ]; 

%  COMPUTATION  OF  KALMAN  GAIN 

vresid  =  (H*P*(H')  +  R);  %  variance  of  the  residual 

G  =  P*(H')  /  vresid;  %  Kalman  gain 


6" 


%  COMPUTATION  OF  UPDATED  ERROR  COVARIANCE 
P  =  (I  -  G*H)*P; 

%  COMPUTATION  OF  RESIDUAL 
resid  =  zobs(: ,k+l)  -  zhat; 

%  IF  RESIDUAL  IS  TOO  BIG,  THEN  RESET  P  AND  DO  CALCULATIONS  AGAIN, 
if  ( abs( resid( 1) )  >  1. 0*sqrt( abs( vresidC 1 , 1) ) )  ... 

]  abs( resid(2) )  >  1.  0*sqrt(abs(vresid(2,2)))  ... 

&  nbad  —  0) 
nbad  =  1; 
k  =  k  - 1 ; 

P  =  PO;  %  reset  P 

else 

%  COMPUTATION  OF  UPDATED  ESTIMATE 
xte(: ,k+l)  =  xte(: ,k+l)  +  G*resid; 

%  ERROR  ELLIPSOID  PLOTTING  ROUTINE 
M  =  (xte(l,k+l)  xte(3,k+l)] ' ; 

K  =  [  P(l,l)  P( 1,3) 

P( 3 , 1 )  P( 3 ,3)  ]; 

[xp,yp]  =  errellip(M,K,. 60,25,0); 
eex( : , k)  =  xp; 
eey( : ,k)  =  yp; 

%  PREDICTED  (FUTURE  "TRACK)  USING  KALMAN  ESTIMATE 
%  (number  of  predictions  desired  input  from  marine. m) 
tpred(: ,1)  =  xte(: ,k+l); 
acc  =  zeros( 2,1); 

for  1  =  lrkpred  %  kpred  is  #  of  predictions 

if  k  >=  5 

accx  =  (xte( 2 ,k+l) -xte(2,k) )/dt; 
accy  =  (xte( 4 ,k+l) -xte(4,k) )/dt; 
acc  =  [ accx; accy] ; 
if  accx  >=  5 
break 

acc  =  [  0;  0]  ; 
elseif  accy  >=  5 
break 

acc  =  [  0;  0J  ; 

end 

end 

tpred(: ,1+1)  =  phi*tpred(: , l)+del*acc; 

end 

nbad  =  0; 

end 

%  PLOT  OBSERVED  AND  ESTIMATED  TRACKS  (TARGET  AND  OBSERVER) 

plot(xfol(l,l:k+l),xfol(3,l:k+l),'xw',xte(l,l:k+l), 

xte( 3 , 1: k+1) , '+w' , .  .  . 

xfo2(l,l:k+l),xfo2(3,l:k+l),'xw', zpos( 1 , 1: k+1) ,zpos( 2 , 1: k+1) , ' ow' , .  .  . 
xto( 1 , 1: k+1 ) ,xto( 3, 1:  k+1 ) , ' .  b ' , tpred( 1 , :  ) , tpred(3, :  ) , ' *g' , .  .  . 
eex,eey , ' -r' ) .grid 

title( ’ACTUAL/OBSERVED/ESTIMATED/PREDICTED  TGT  TRKS 


USING  RNG/RNG  OBS' ) ,. . . 

xlabelC DISTANCE  X' ) ,ylabel( 'DISTANCE  Y’) 

end 

meta  rngrng 
diary  off 


X  JOHN  A.  HUCKS  II 
X  FILE  NAME  :  BRGRNG.  M 

%  EXTENDED  KALMAN  FILTER  USING  BEARING/RANGE  MEASUREMENTS 

X  THIS  PROGRAM  SIMULATES  THE  TRACK  GENERATED  BY  AN  EXTENDED  KALMAN 
X  FILTER  WITH  INPUTS  FROM  A  FUSED  SENSOR  ARRAY,  AS  IT  TRACKS  A 
%  POTENTIAL  TARGET  ACROSS  AN  X-Y  GRID  SPACE  (PLRS  FLAT  EARTH 
%  PROJECTION). 


clg 

clear  eex  eey  xto 
! erase  brgrng. met 
diary  brgrng 


%  STATE  EQUATIONS 

X  OBSERVATION  TIME  INTERVAL  (input  from  marine,  m) 

°o  STATE  TRANS  I  ST  I  ON  MATRIX 
phi  =  [  1  dt  0  0 

0  10  0 

0  0  1  dt 

0  0  0  1  ]  ; 

%  SYSTEM  NOISE  COEFFICIENT  MATRIX 
del  =  [  dt*dt/2  0 

dt  0 

0  dt*dt/2 

0  dt  ]; 

To  STATE  VARIABLES,  FILTER  ESTIMATES,  TIME,  AND  OBSERVED  DATA 
%  NUMBER  OF  OBSERVATIONS  (input  from  marine,  m) 


%  ESTIMATED  TARGET  STATE 

xte  =  zeros(4,kmax);  X  estimated  target  state 

X  OBSERVER  (SENSOR)  INITIAL  POSITIIONS  AND  VELOCITIES  (input  from  marine,  m) 

X  OBSERVER  SPEED  (input  from  marine,  m) 

%  INITIAL  TIME  SETUP 
Time  =  zeros( 1 ,kmax) ; 

X  ACTUAL  TRACK  POSITIONS  (tgt  vector  input  from  marine,  m) 
x  =  zeros( 1 ,kmax); 
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y  =  zeros( 1 ,kmax); 

T  =  0; 

for  k  =  1 

x(k)  =  tgt( 1,1); 
y(k)  =  tgt(  4 ,  1 ) ; 

end 

for  k  =  2: kmax 

T  =  T+dt; 

x(k)  =  (0.5*tgt(3,l)*(TA2))+(tgt(2,l)*T)+(tgt(l,15); 
y(k)  =  ( 0.  5*tgt(6 , 1),v(Ta2)  )+(tgt(5 ,  l)*T)+(tgt(4, 1)); 

end 

zpos  =  [  x 

y  ]; 

%  INPUT  OBSERVATION  VALUES  (BEARING/RANGE): 

xfoa  =  zeros (4, kmax); 

xfoa( : ,1)  =  xfol(: ,1); 

xfol  =  xfoa; 

xfob  =  zeros( 4 , kmax) ; 

xfob( : , 1)  =  xfo2( : , 1 ) ; 

xfo2  =  xfob; 

xfor  =  zeros(4 ,kmax); 

zobr  =  zeros( 1: kmax); 

rngl  =  zeros( 1:  kmax) ; 

rng2  =  zercs( 1: kmax); 

deltlx  =  zeros( 1: kmax); 

delt2x  =  zeros( 1: kmax); 

deltly  =  zeros( 1: kmax); 

delt2y  =  zeros( 1: kmax) ; 

thtal  =  zeros( 1: kmax); 

thta2  =  zeros( 1: kmax); 

for  k  =  1: kmax 

rngl(k)  =  sqrt( ( ( zpos( 1 ,k) -xfol( 1 ,k) ).  a2)  +  ... 

( ( zpos( 2,k)-xfol(3,k)).  *2) ); 
rng2(k)  =  sqrt( ( ( zpos( 1 ,k) -xfo2( 1 ,k) ) .  a2)  +  ... 

( ( zpos( 2,k)-xfo2(3,k)). a2)); 
deltlx(k)  =  zpos( 1 ,k) -xfol( 1 ,k); 
deltlylk)  =  zpos(  2  ,k) -xfol(  3 ,k); 
delt2x(k)  =  zpos( 1 ,k) -xfo2( 1 ,k); 
delt2y(k)  =  zpos( 2 ,k) -xfo2( 3 ,k); 
thtal(k)  =  atan2( delt lx(k) ,delt ly( k) ) ; 
thta2(k)  =  atan2^delt2x(k) ,delt2y(k) ); 

end 

zobr  =  (  thtal; rngl; thta2;  rng2  ]; 

%  MEASUREMENT  REFERENCE  CONVERSION  ROUTINE: 
for  cnt  =  1 

xtl  =  tgt( 1,1); 

xt2  =  tgt( 1,1); 

ytl  =  tgt( 4,1); 

yt2  =  tgt( 4 , 1 ); 

xtr  =  (xtl+xt2)/2; 

ytr  =  (ytl+yt2)/2; 

delxr  =  xtr(l,cnt); 

delyr  =  ytr( 1 ,cnt); 

thtar(l,cnt)  =  atan2(delxr , delyr); 
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rngr(l,cnt)  =  sqrt((delxr  2)+(delyrA2) ); 

end 

for  cnt  =  2:  kmax 

xtl(l,cnt)  =  zobr( 2 ,cnt)*sin(zobr( 1 ,cnt) ); 
ytl(l,cnt)  =  zobr(2,cnt)*cos(zobr( l,cnt)); 
xt2(l,cnt)  =  zobr(4,cnt)*sin(zobr(3,cnt)); 
yt2(l,cnt)  =  zobr(4,cnt)*cos(zobr(3,cnt)); 
xtr(l,cnt)  =  (xtl( l,cnt)+xt2( l,cnt))/2; 
ytr(l,cnt)  =  (yt 1( 1 , cnt )+yt2( 1 , cnt ) )/2; 
delxr  =  xtr(l.cnt); 
delyr  =  ytr(l,cnt); 
thtar(l.cnt)  =  atan2(delxr , delyr); 
rngr(l,cnt)  =  sqrt((delxr  2)+( delyr a2) ); 

end 


X  CONVERTED  OBSERVATION  MATRIX:  (CONVERSION  OBSERVER  TAKEN  AS  THE  ORIGIN) 
X  OBSERVED  TARGET  STATE 


xto(: ,1)  =  [  tgt(l,l) 

tgt( 2,1) 

tgt(4, 1) 
tgt( 5 , 1)  ]  ; 
zobs  =  [  thtar 
rngr  ] ; 

xtol  =  zeros(4,kmax); 
xtol( 1 , :  )  =  zpos(  1 , :  ); 
xtol( 3 , :  )  =  zpos( 2  , :  ); 
xtol( : ,1)  =  xto( : , 1 ); 
xto  =  xtol; 


%  (km  in  x  direction) 

%  (km/hr  in  x  direction) 
%  (km  in  y  direction) 

%  (km/hr  in  y  direction) 


6o  STATE  EXCITATION  AND  MEASUREMENT  ERROR  COVARIANCE  MATRICES 
varv  =5.0;  X  variance  of  linear  acceleration 

varth  =1.0;  X  variance  of  angular  acceleration 

R  =  [  0. 0005  0 

0  0. 005  ] ;  X  measurement  noise 


?;  INPUT  OBSERVATION  VALUES  WITH  NOISE  ADDED 
rand( ' normal ' ) 

noise  =  sqrt(R( 1 , 1 ) )*rand( zobs) ; 


X  INITIAL  KALMAN  MATRICES  AND  OBSERVATIONS 

j  s  eye ( 4 ) y 

X  initial’ KALMAN  P  MATRIX 

P0  =  [  700  000  X  reset  value  of  error  covariance 

0  700  0  0 

0  0  700  0 

0  0  0  700  ]; 

X  REINITIALIZING  P  MATRIX 

P  =  P0; 

%  INITIAL  KALMAN  ESTIMATE 

xte( : , 1)  =  [  xto( 1,1) 
tgt( 2,1) 
xto( 3,1) 
tgt(5,l)  ]; 

tpred  =  xte( : , 1); 
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%  SIMULATION  OF  FUSED  SENSOR  OPERATION  USING  EXTENDED  KALMAN  FILTER 
%  NUMBER  OF  BAD  OBSERVATIONS 

nbad  =0;  %  initial  setting  bad  obs.  cntr. 

k  =  0; 

for  kl=l:  50*kmax 

k  =  k  +  1;  X  actual  loop  counter 

if  (k  =  km ax)  X  check  for  termination 

break 

end 


%  UPDATE  OBSERVER'S/FAC'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 
way  =  pi/2; 
else 


way  =  -pi/4; 


end 
if  spd  = 
xfol( 1 
xfol( 3 
xfo2( 1 
xfo2( 3 
else 

xfol( 1 
xfol( 2 
xfol( 3 
xfol(4 
xfo2( 1 
xfo2( 2 
xfo2( 3 
xfo2(4 

end 


0 

,k+l) 

>k+l) 

,k+l) 

jk+1) 

,k+l) 

,k+l) 

,k+l) 

i  k+1 ) 

,k+l) 
,k+l) 
»k+l ) 
»k+l) 


xfol( 1 ,k) 
xfol( 3 , k) 
xfo2(l,k) 
xfo2( 3  ,k) 


xfol(l,k)  +  spd*sin(way)*dt; 
spd*sin(way); 

xfol(3,k)  +  spd*cos(way)*dt; 
spd*cos(way); 

xfo2(l,k)  +  spd*sin(way)*dt; 
spd*sin(way); 

xfo2(3,k)  +  spd*cos(way)*dt; 
spd*cos(way); 


%  PROJECT  AHEAD  KALMAN  STATE  ESTIMATE 
xte(:  ,k+l)  =  phi^xteC:  ,k); 

o/  UPDATE  Q 

vt  =  sqrt( (xte( 2 ,k+l) ) a2  +  (xte(4 ,k+l) ) a2); 
qll  =  varv  ,v  (xte(2,k+l)  /  vC)a2  +  varth  *  (xte(4,k+l)) a2; 
q22  =  varv  *  (xte(4,k+l)  /  vt)A2  +  varth  *  (xte( 2 ,k+l) ) a2; 
ql2  =  xte(2,k+l)  *  xte(4,k+l)  *  ((varv/vt)A2  -  varth  ); 
q21  =  ql2; 

Q1  =  (  qll  ql2 

q21  q22  1; 

Q  =  del*Ql*(dei  ); 

X  PROJECT  ERROR  COVARIANCE 
P  =  phi*P*(phi' )  +  Q; 

%  UPDATE  H 

deltx  =  xte(l,k+l); 

deity  =  xte(3,k+l); 

r2  =  (deltx)  2  +  (deity)  2; 

r  =  sqrt(r2); 

H  =  [  delty/r2  0  -deltx/r2  0 

deltx/r  0  delty/r  0  ]  ; 
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%  GET  NEW  MEASURED  ESTIMATE 

delx  =  xte( 1 ,k+l); 

dely  -  xte(3,k+l); 

zhatl  =  atan2(delx,dely); 

zhat2  =  sqrt(delxA2  +  delyA2); 

zhat  =  [  zhatl 

zhat2  ]  ; 

%  COMPUTATION  OF  KALMAN  GAIN 

vresid  =  (H*P*(H')  +  R);  %  variance  of  the  residual 

G  =  P*(H')  /  vresid;  %  Kalman  gain 

%  COMPUTATION  OF  PROJECTED  ERROR  COVARIANCE 
P  =  (I  -  G*H)*P; 

%  COMPUTATION  OF  RESIDUAL 
resid  =  zobs(: ,k+l)  -  zhat; 

%  IF  RESIDUAL  TOO  BIG,  RESET  P  AND  DO  CALCULATIONS  AGAIN, 
if  ( abs( res  id ( 1 ) )  >  1. 0*sqrt( abs( vresid(  1 , 1) ) )  ... 

]  abs( resid( 2) )  >  1. 0*sqrt( abs( vresid( 2 , 2) ) )  ... 

&  nbad  <  2) 
nbad  =  nbad  +  1; 
k  =  k-1; 

P  =  PO;  %  reset  P 

else 

%  COMPUTATION  OF  UPDATED  ESTIMATE 
xte(: ,k+l)  =  xte(: ,k+l)  +  G*resid; 

%  ERROR  ELLIPSOID  PLOTTING  ROUTINE 
M  =  [xte(l,k+l)  xte( 3 ,k+l)] ’ ; 

K  =  [  P(l,l)  P ( 1,3) 

P( 3 , 1 )  P( 3 , 3)  ]; 

[xp,yp]  =  errellipCM.K, .  60,25,0); 
eex( :  ,k)  =  xp; 
eey(:  ,k)  =  yp; 

“o  PREDICTED  (FUTURE  TRACK)  USING  KALMAN  ESTIMATE 
%  (number  of  predictions  desired  input  from  marine,  m) 
tpred(: ,1)  =  xte(: ,k+l); 
acc  =  zeros( 2,1); 

for  1  =  1:  kpred  %  kpred  is  #  of  predictions 

if  k  >=  5 

accx  =  (xte( 2 ,k+l) -xte( 2 ,k) )/dt; 
accy  =  (xte(4,k+l)-xte(4,k))/dt; 
acc  =  [ accx; accy]  ; 
if  accx  >=  5 
break 

acc  =  zeros(2,l); 
elseif  accy  >=  5 
break 

acc  =  zeros( 2,1); 

end 

end 

tpred( : ,1+1)  =  phi*tpred( : , l)+del*acc; 


end 

nbad  =  0; 


end 

%  PLOT  OBSERVED  AND  ESTIMATED  TRACKS  (TARGET  AND  OBSERVER) 
plot(xfo 1(1,1: k+l),xfol(3,l:k+l),'xw' ,xfo2( 1 , 1:  k+1) , 
xfo2(  3 , 1:  k+1) , 'xw' ,.  .  . 

xto( 1 , 1:  k+1) ,xto( 3, 1:  k+1)  , ' .  b* )xte(l,l:  k+1) ,xte(3, 1:  k+1) 
zpos( 1 , 1:  k+1) ,zpos(2 , 1: k+1) , ' ow' ,tpred( 1,: ) ,tpred(3,:  )  ,  '*g'  . 

eex,eey, ' -r ’ ) .grid 

title( 'ACTUAL/ OBSERVED/ESTIMATED/PREDICTED  TGT  TRKS 
USING  BRG/RNG  OBS' ),. . . 

xlabeK 'DISTANCE  X' ) ,ylabel( ' DISTANCE  Y') 

end 


diary  off 
meta  brgrng 


°o  JOHN  A.  HUCKS  II 
%  FILE  NAME  :  DOPPLER.  M 

%  EXTENDED  KALMAN  FILTER  PROGRAM  USING  A  DOPPLER  SHIFTED  FREQUENCY 
°o  SIGNAL  AND  THE  BEARING  TO  THAT  SIGNAL. 

THIS  PROGRAM  SIMULATES  THE  TRACK  GENERATED  BY  AN  EXTENDED  KALMAN 
%  FILTER,  WITH  INPUTS  FROM  A  FUSED  SENSOR  ARRAY  AS  IT  TRACKS  A  TARGET 
ACROSS  AN  X-Y  GRID  SPACE  (PLRS  FLAT  EARTH  PROJECTION). 


clg 

clear  eex  eey  xto 
! erase  doppler.met 
diary  doppler 

%  STATE  EQUATIONS 

%  OBSERVATION  TIME  INTERVAL  (input  from  marine. m) 
%  STATE  TRANSITION  MATRIX 


=  [  1  dt  0 

0 

0 

0 

1  0 

0 

0 

0 

0  1 

dt 

0 

0 

0  0 

1 

0 

0 

0  0 

0 

1  ); 

SYSTEM  NOISE  COEFFICIENT 

MATRIX 

=  [  dt 

dt*dt/2 

0 

0 

0 

0 

dt 

0 

0 

0 

0 

0 

dt 

dt*dt/2 

0 

0 

0 

0 

dt 

0 

0 

0 

0 

0 

dt  ]; 

%  STATE  VARIABLES,  FILTER  ESTIMATES,  TIME,  ANT  OBSERVED  DATA 
%  NUMBER  OF  OBSERVATIONS  (input  from  marine. m) 
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%  ESTIMATED  TARGET  STATE 

xte  =  zeros(5 ,kmax);  %  initial  est.  target  state 

?  OBSERVER  (SENSOR)  INITIAL  POSITIONS  AND  VELOCITIES 

%  (input  from  marine,  m) 

xfoa  =  zeros( 4 ,kmax) ; 

xfoa( : , 1)  =  xfol( : , 1); 

xfol  =  xfoa; 

%  OBSERVER  SPEED  (input  from  marine. m) 

%  INITIAL  TIME  SETTING 
Time  =  zeros( 1 ,kmax); 

ACTUAL  TRACK  POSITIONS  (input  from  marine,  m) 
x  =  zeros( 1 ,kmax); 
y  =  zeros( 1 ,kmax); 

T  =  0; 
for  k  =  1 

x(k)  =  tgt( 1,1); 
v(k)  =  tgt( 4 , 1 ); 
fo(k)  =tgt( 7,1); 

end 

for  k  =  2: kmax 

T  =  T+dt; 

x(k)  =  (0.5*tgt(3,l)*(T'.2))+(tgt(2,l).*T)+(tgt(l,l))j 
vx(k)  =  ( tgt( 3 , 1 )*T)+tgt( 2 , 1 ); 

y(k)  =  (0.  5,vtgt(  6 , 1  )*(  Ta2  ) )+( tgt(  5 , 1).  *T)+(  tgt(4 , 1 ) ) ; 
vy ( k )  =  ( tgt( 6 , 1 )*T)+tgt( 5,1); 
fo(k)  =  tgt( 7 , 1); 

end 

zpos  =  [  x 

YX 

y 

vy 

fo  ] ;  %  rest  frequency  xmtr  (Hz) 


o 

'O 

o 

/O 


INPUT  OBSERVATION  VALUES  (BEARING/FREQUENCY) 
INITIAL  OBSERVED  TARGET  STATE 


( km  in 
(km/hr 
( km  in 
( km/hr 


x  direction) 
in  x  direction) 
y  direction) 
in  y  direction) 


%  rest  frequency  xmtr  (Hz) 


xto  =  zeros; 5 , kmax); 
xto( :  , 1)  =  [  tgt( 1,1) 
tgt(2,l) 
tgt(4, 1) 
tgt( 5 ,1) 
tgt(7,l)  ] ; 

deltlx  =  zpos(l,:)  -  xfol(l,:); 
deltly  =  zpos(3,:  )  -  xfol(3,:  ); 
brgl  =  atan2( deltlx , deltly); 
vp  =  1.  0789e9; 
for  k  =  1: kmax 

f(k)  =  (zpos(5 ,k)*vp)/( vp+( ( zpos( 1 ,k)*zpos(2 ,k) )+(zpos(  3,k)*. 
zpos (4 ,k) ) )/sqrt( ( zpos( 1 ,k) A2+zpos( 3 ,k) a2) ) ) ; 


% 

% 


bearing  thtal 
spd  light  (km/hr) 


end 

zobs  =  [  brgl;  f]  ; 


xtol  =  zeros(5 ,kmax); 


xtol(l,:)  =  zpos(l,:); 
xtol(2,:)  =  zpos  (  2 , :  ) ; 
xtol(3,:)  =  zpos(3,:); 
xtol(4,:)  =  zpos ( 4 , : ) ; 
xtol(5,:)  =  zpos( 5  , :  ) ; 
xtol( : , 1 )  =  xto( : , 1 ) ; 
xto  =  xtol; 

%  STATE  EXCITATION  AND  MEASUREMENT  ERROR  COVARIANCE  MATRICES 
varv  =  1. 005;  %  variance  of  linear  acceleration 

varth  =  5. 05;  %  variance  of  angular  acceleration 

vfreq  =  1.  0;  %  variance  of  the  rest  frequency 

R  =  [  le-11  0 

0  5e-ll  ]  ;  %  measurement  noise 

%  INPUT  OBSERVATION  VALUES  WITH  NOISE  ADDED 
rand( ' normal ' ) 

zobs  =  zobs  +  sqrt(R( 1, l))*rand(zobs); 

%  INITIAL  KALMAN  MATRICES  AND  OBSERVATION 
I  =  eve(5); 

°o  INITIAL  KALMAN  P  MATRIX 

P0  =  [  300  0  0  0  C  °0  reset  value  of  Error  Covariance 

0  300  0  00 

0  0  300  0  0 

0  00  300  0 

0  0  0  0  300  ]  ; 

%  REINITIALIZING  P  MATRIX 
P  =  P0; 

%  KALMAN  ESTIMATE 

xte(: ,1)  =  [  xto(l,l)  %  (km  -  x  direction) 

tgt(2,l)  %  (km/hr) 

xto(3,l)  %  (km  -  y  direction) 

tgt(5,l)  %  (km/hr) 

tgt(7,l)  ];  %  rest  frequency  xmtr  (Hz)_ 

tpred  =  xte( : , 1 ) ; 

%  SIMULATION  OF  KALMAN  FILTER  OPERATION 
%  NUMBER  OF  EAD  OBSERVATIONS 

nbaJ  -  0;  %  initial  setting  bad  obs.  cntr. 

%  SIMULATION  COUNTER  LOOP 
k  =  0; 

for  kl=l:  10-vkmax 

k  =  k  +  1;  %  actual  loop  counter 

if  (k  —  kmax)  %  check  for  termination 

break 

end 

%  UPDATE  FORWARD  OBSERVER'S/  FAC'S  POSITION  (IF  THEY  ARE  MOVING) 
if  k  <=  3 
way  =  pi/2; 
else 
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way  =  -(3*pi/4); 

end 

if  spd  =  0 

xfol(l,k+l)  =  xfol(l,k); 
xfol(3,k+l)  =  xxc’.O.k), 
else 

xfol(l,k+l)  =  xfol(l,k)  +  spd*sin(way)*dt; 
xfol(2,k+l)  =  spd*sin(way); 
xfol(3,k+l)  =  xfol(3,k)  +  spd*cos(way)*dt; 
xfol(4,k+l)  =  spd*cos(way); 

end 

%  PROJECT  AHEAD  KALMAN  STATE  ESTIMATE 
xte(:  ,k+l)  =  phi*xte(:  ,k); 

%  UPDATE  Q 

vt  =  sqrt((xte(2,k+l))A2  +  (xte(4,k+l))A2); 

sigxdd2  =  ((xte(2,k+l)/vt)A2*varv)  +  ( (xte(4,k+l) A2)*varth); 

sigydd2  =  ( (xte( 4 ,k+l)/vt) a 2*varv)  +  ( (xte( 2 Jk+l)A2)*varth); 

s igxvdd  =  ( xte( 2 ,k+l)*xte( 4 ,k+l) )*( ( varv/(vt*vt) ) -varth); 

qll  =  ((dt*dt)/2) ■ 2*sigxdd2; 

ql2  =  ((dt*dt*dt)/2)*sigxdd2; 

ql3  =  ((dt*dt)/2)*sigxvdd; 

q;4  =  ( (dt*dt*dv)/2)*sigxydd; 

q!5  =  0; 

q21  =  q!2; 

q22  =  (  dt-dt  ),vsigxdd2; 

c23  =  ((dt*dt*dt)/2)*sigxydd; 

q2-*  =  (  dt*dt  ),vs  igxydd; 

q25  =  0; 

q31  =  q i 3 ; 

q32  —  q  2  3  * 

q23  =  ((dt*dt)/2)-2*sigydd2; 

q34  =  ( (dt*dt,vdt)/'2)"Sigvdd2; 

q35  =  0; 

q4  1  =  q  14; 

q42  =  q24; 

q43  =  q34; 

q44  =  (dt*dt)*sigydd2; 

q45  =  0; 

q51  =  ql5; 

q52  =  q25 ; 

q53  =  q  3  5 ; 

q54  =  q45; 

q55  =  (dt*dt)*vfreq; 

Q1  =  [  qll  ql2  ql3  ql4  ql5 
q21  q22  q23  q24  q25 
q3 1  q32  q33  q34  q35 
q41  q42  q43  q44  q45 
q51  q52  q53  q54  q55  ]  ; 

Q  =  del*Ql*( del ' ); 

%  PROJECT  ERROR  COVARIANCE 
P  =  phi*P-(phi ' )  +  Q; 
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%  UPDATE  H 

rl2  =  sqrt( (xte( 1 ,k+l) A2)+(xte( 3 ,k+l) a2) ); 
r32  =  (sqrt( (xte( 1 ,k+l) A2)+(xte( 3 ,k+l) a2) ) ) a3; 
deltxvy  =  xte( 1 ,k+l)*xte(4,k+l)  -  xte( 3 ,k+l )*xte( 2 ,k+l); 
deltyvx  =  xte( 3 ,k+l)*xte( 2 ,k+l)  -  xte( 1 ,k+l)*xte(4,k+l); 
fkvp  =  xte(5 ,k+l)*vp; 

hll  =  -(xte( 3 ,k+l) /( (xte( 1 ,k+l) a  2)+(xte( 3 ,k+l) a2) ) ); 
hl2  =  0; 

hl3  =  xte(l,k+l)/((xte(l,k+l)A2)+(xte(3.k+l)A2)); 
hl4  =  0; 
hl5  =  0; 

h21  =  -((zobs(2,k+l)A2)/fkvp)*(xte(3,k+l)*deltyvx)/r32; 
h22  =  -((zobs(2,k+l)A2)/fkvp)*(xte(l,k+l)/rl2); 
h23  =  -(zobs( 2 ,k+l)/fkvp)*(xte( 1 ,k+l)*deltxvy)/r32; 
h24  =  -( ( zobs( 2 ,k+l) A2)/fkvp)*(xte( 3 , k+1) /r 12); 
h25  =  zobs( 2 ,k+l ) /xte( 5 ,k+l) ; 

H  =  [  hll  hl2  h 1 3  hl4  hl5 

h21  h22  h23  h24  h25  ] ; 

%  GET  NEW  MEASURED  ESTIMATE 
deltlx  =  xte(l,k+l)  -  xfol(l,k+l); 
deltly  =  xte(3,k+l)  -  xfol(3,k+l); 

zhatl  =  atan2(deltlx, deltly);  %  bearing  estimate  thtal 

zhat2  =  (xte1-  5  , k+1  )*vp) /( vp+(  (xte(  1  ,k+l)*  %  frequency  estimate 

xte( 2 , k+1 ) )+( xte( 3 , k+1 )*.  .  . 

xte(4,k+l)))/sqrt((xte(l,k+l) '2+xte( 3 ,k+l) a2) ) ); 
zhat  =  [  zhatl 

zhat2  ] ; 

%  COMPUTATION  OF  KALMAN  GAIN 

vresid  =  ( H'rP'r( H '  )  +  R);  »  variance  of  the  residual 

G  =  P*(H' )  /  vresid;  %  Kalman  gain 

%  COMPUTATION  OF  UPDATED  ERROR  COVARIANCE 
P  =  (I  -  G*H)*P; 

%  COMPUTATION  OF  RESIDUAL 
resid  =  zobs(: ,k+l)  -  zhat; 

°o  IF  RESIDUAL  IS  TOO  BIG,  THEN  RESET  P  AND  DO  CALCULATIONS  AGAIN, 
if  (abs(resid(  lj )  >  1.  0'vsqrt(  abs(vres  id(  1 , 1) ) )  ... 

]  abs( resid( 2) )  >  1.  0*sqrt( abs( vres id( 2 , 2) ) )  ... 

&  nbad  =  0) 
nbad  =  1; 
k  =  k- 1; 

P  =  PO;  %  reset  P 

else 

%  COMPUTATION  OF  UPDATED  ESTIMATE 
xte(: ,k+l)  =  xte(: ,k+l)  +  G*resid; 

%  ERROR  ELLIPSOID  PLOTTING  ROUTINE 
M  =  (xte(l,k+l)  xte(3,k+l)]; 

K  =  [  P(l,l)  P( 1 , 3) 


78 


**  fr?  i *  O'"  S'?  3 


P( 3 j 1)  P( 3 , 3)  3; 

[xp,yp]  =  errellip(M,K,. 60,25,0); 
eex( : ,k)  =  xp; 
eey(: ,k)  =  yp; 

X  PREDICTED  (FUTURE  TRACK)  USING  KALMAN  ESTIMATE 
tpred(: ,1)  =  xte(: ,k+l); 
acc  =  zeros(5,l); 
for  1  =  1:  kmax 
if  k  >=  3 

acc( 2,1)  =  (xte(2,k+l)-xte(2,k))/dt; 
acc(4,l)  =  (xte(4,k+l)-xte(4,k))/dt; 
if  acc( 2,1)  >=  5 
break 

acc  =  zeros(5,l); 
elseif  acc(4,l)  >=  5 
break 

acc  =  zeros( 5,1); 

end 

end 

tpred(: ,1+1)  =  phi*tpred(: , l)+del*acc; 

end 

nbad  =  0; 
end 

%  PLOT  OBSERVED  AND  ESTIMATED  TRACKS  (TARGET  AND  OBSERVER) 
plot(xfol(l,l:k+l),xfol(3,l:k+l),'xw' ,xto( 1 , 1: k+1 )  , 
xto( 3,1: k+1 ) , ' . b' , . . . 

xte( 1 , 1:  k+1 ) ,xte( 3 , 1:  k+1) f ' +w'  ,zpos( 1 , 1: k+1) ,zpos( 3 , 1: k+1 ) , ’ ow ' , . 
tpred( 1 ,:  ) ,tpred( 3,:  ) , '*g  ,eex,eey, ' -r’ ) ,grid 
titlef 'ACTUAL/OBSERVED/ESTIMATED/PREDICTED  TGT  TRKS 
USING  DOPPLER  '),... 

xlabelC ’DISTANCE  X’ ) ,ylabel( ’DISTANCE  Y' ) 

end 

meta  doppler 


function  [x,y]  =  errellip(M,K,Pr,n,MD) 

© 

%  errellip(M,K,Pr ,n,MD) 

O' 

o 

This  function  takes  the  following  arguments: 

M  Mean  Vector  (2x1) 

K  Covariance  Matrix  (2x2) 

Pr  Probability  (0  ..  1) 

n  Number  of  points  to  compute 

MD  Compute  by  Mahalanobis  Distance  vice  probability 
0  =  Probability 
1  =  Mahalanobis  Distance 

and  returns  x  and  y  vectors  of  the  confidence  ellipse  for  the 
X  given  probability  or  Mahalanobis  Distance. 

%  Stephen  L.  Spehn  15  Nov  1989 
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if  nargin  =  5 

error( ' Incorrect  number  of  arguments  to  errellip. '); 
end; 

if  MD  =  1 

c  =  abs(Pr); 
else 

Pr  =  max(min( . 995  ,Pr ) , . 005 ) ; 


%  Cubic  spline  fit  for  the  ellipse  constant. 

%  Using  this  method  is  a  compromise  between  speed  and  accuracy. 


pp  =  [  12;  . 005;  . 01; 

.95;  .975;  .99; 
4. 190809 19 7869072 e+1; 
3. 970929262330003; 

1. 62882241 1 110 34e+l; 

4. 17 6985 755 22 307 9e+2; 
-3. 810356328008524e-l; 
5. 444089 169224 142e-l; 
2. 55641940780766; 

4. 6070422925247e+2; 

2. 020857475864527; 

2. 122852230998054; 

3. 861381142104123; 


025;  .05;  .1;  .25;  .5; 
995;  4.0; 

4. 190809 1978 72625e+l; 
2. 15930349191602; 

8.  24 375856514019 e+1; 

2. 087990965023806e+5; 
2. 47585746879301 le-1; 
1. 140048306271903; 

1.  477458749113522 e+1; 
4.  920316224166436e+2; 
2. 02019022643493; 

2. 20707509215777; 

8. 195632865839841; 

6.  76397289507 145 8e+l; 
.211;  .575; 

.  38;  9.  21  ]  ; 


.75;  .9; 

-2.  118721291999464e+l; 
5.  9557937356473 19e* 1; 
2.  72555 1521454689e+3; 
2.  087990965023842e+5; 
2.  133449885921905; 

2.  111734877634124; 

5.  187150103426589e+l; 
9.  887990965023757 e+3; 
2.  055905760926949; 

2.  694842569743673; 

1.  81925461 4465004 e+1; 

2.  2 334006 7 7 623 2 le+2; 


4. 382133265898673e+l; 
.01;  .0201;  .0506;  .103; 
1.39;  2.77;  4.61;  5.99; 


c  =  ppva 1( pp , Pr  J ; 
end; 


°o  Get  Eigenvectors ,  Eigenvalues,  and  translated  variances 
[EveCjEval)  =  eig(K); 
sigx  =  scrt ( Eval( 1,1)); 
sigy  =  sqrt( Evalf 2 , 2 ) ); 


°o  Parameterized  ellipse  equations 
t  =  0: ( 2*pi/n): (2*pi); 
xv  =  sigx*c*cos ( t ) ; 
yv  =  sigy*c*sin(t); 


%  Translate  back  to  the  original  coordinates 
x  =  ( xv*Evec( 1 , 1 )  +  yv*Evec(l,2)  +  M(l))'; 
y  =  ( xv*Evec(  2 , 1 )  +  yv-':Evec(  2,2)  +  M C  2 ) )  ’ ; 


function  y  =  reshape(x,m,n) 

^RESHAPE  RESHAPE(X,M,N)  returns  the  M-by-N  matrix  whose  elements 
%are  taken  columnwise  from  X.  An  error  results  if  X  does 
%not  have  M*N  elements. 
mm,nn  =  size(x); 
if  mm*nn  =  m*n 

error( 'Matrix  must  have  M*N  elements.') 
end 


80 


LIST  OF  REFERENCES 


1.  Sorenson,  H.W.,  "Kalman  Filtering  Techniques",  Advanced  Control  Systems ,  Vol. 

3.  Chap.  5,  1%6. 

2.  Tzu,  Sun.  The  Art  of  War,  Clavell,  J.,  ed.,  Delacorte  Press,  NY,  1983. 

3. 

U.S.  Army  Communications  and  Electronics  Command,  Contract  No. 
DAABu7-S3-C-Jo31.  PLRS  System  Technical  Description ,  Fort  Monmouth,  NJ, 
April  lVSo. 

4.  Mitschang.  GAV  .  An  Application  of  Nonlinear  Filtering  Theory'  to  Passive  Target 
Location  and  Tracking'.  Doctoral  Dissertation,  Nava]  Postgraduate  School. 
Monterey.  CA.  June  iv~4. 

5.  Collin.  R  E.  Antennas  and  Radio  IFair  Propagation,  p.  307,  McGraw-Hill,  NY, 

1 955 . 

6.  Gclb.  A  .  and  others.  Applied  Optima!  Estimation,  Gelb,  A.,  ed.,  p.  182,  MIT  Press, 
MA.  19'4. 

7.  Therrien.  C.W..  Decision  Estimation  and  Classification,  John  Wiley  and  Sons,  Inc., 
NY.  I  VST 

8.  Baheti.  R.S..  "Efficient  Approximation  of  Kalman  Filter  for  Target  Tracking", 
IEEE  Trans,  on  Aerospace  and  Electronic  Systems,  Vol.  AES-22,  No.  1,  pp.  8-14, 

1956. 


82 


INITIAL  DISTRIBUTION  LIST 


No.  Copies 

1.  Defense  Technical  Information  Center  2 

Cameron  Station 

Alexandria,  VA  22304-6145 

2.  Library,  Code  0142  2 

Naval  Postgraduate  School 

Monterey,  CA  93943-5002 

3.  Commandant  of  the  Marine  Corps  1 

Code  TE  06 

Headquarters.  U.  S.  Marine  Corps 
Washington,  D.C.  20380-0001 

4.  Chairman.  Code  62  1 

Department  of  Electrical  and  Computer  Engineering 
Na\al  Postgraduate  School 
Monterey,  CA  93943-5000 

Professor  Harold  A.  Titus.  Code  62Ts 
Department  of  Electrical  and  Computer  Engineering 
Naval  Postgraduate  School 
Monterey.  CA  93943-5000 

6.  Professor  Roberto  Cristi.  Code  62Cx  1 

Department  of  Electrical  and  Computer  Engineering 

Naval  Postgraduate  School 
Monterey,  CA  93943-5000 

7.  Professor  Mostafa  Ghandahari,  Code  53Gh  1 

Department  of  Mathematics 

Naval  Postgraduate  School 
Monterey,  CA  93943-5000 

8.  Commandine  Officer  5 

Attn:  CAPT  J.A.  Hucks  II  USMC 

Marine  Corps  Tactical  Systems  Support  Activity 

Marine  Corps  Base 

Camp  Pendleton,  CA  92055-6045 


83 


r « 


